聲學多普勒速度儀（DVL）是一種測量相對於水底速度的聲納設備。早年DVL一開始被設計主要是用於船舶的定位導航中，但是近年來隨着水下機器人領域，尤其是民用領域的興起（據有關機構測算這一市場規模已達100億美元），DVL的作用顯得愈發重要，因爲水下機器人想要在水下實現自主導航有三樣東西必不可少，一個是姿態測量單元，一個是速度測量單元，一個是位置測量單元。姿態測量單元通常是通過MEMS慣導元件或者光纖慣導元件等測量得到的，而MEMS通常價格非常便宜，非常容易獲得，因此它可以被大量廣泛的用在水下機器人中；位置測量單元可以由GPS、USBL等設備構成，獲取的難度相對容易，成本也不算太高；最後一個速度測量單元目前最有效的手段就是通過DVL來獲取，但其成本高昂，售價一般在十幾萬以上甚至更高，因此在目前民用的水下機器人上很少有它的身影。目前水下機器人市場還沒有完全爆發，主要原因就是相關的傳感器不夠便宜，許多關鍵部件的成本沒有降下來，國內外在這一領域還沒有出現獨角獸。筆者正是意識到了這一點，所以才下定決心來研究DVL，希望有志同道合的朋友能和筆者一起將DVL的成本降下來，讓水下機器人市場徹底爆發的這一天來的更早一些。

1.多普勒測速原理

1.1多普勒效應

多普勒效應是由於聲源與觀測者之間存在着相對運動，使觀測者聽到的聲音頻率不同於聲源所發出聲音頻率的現象。這是由奧地利物理學家Christian Doppler 於十九世紀在聲學領域首先發現的物理學原理。

典型的多普勒效應發生於聲源和觀測者之間。在聲源靜止條件下，當觀測者向着聲源運動時，他收到的聲波頻率高於他相對聲源靜止時收到的聲波頻率；當觀測者遠離聲源而去時，他收到的聲波頻率低於他相對聲源靜止時收到的聲波頻率。聲源於觀測者之間的相對運動速度越大，則所產生的聲波頻率變化也就越大。有關多普勒效應的研究不僅在聲學領域可以看到，利用多普勒效應進行的研究還可擴展到電磁學，光學等其它領域。

下面推導任意方向上的多普勒效應公式。如圖 1 所示，S 爲聲源，D爲觀測者， $\overrightarrow{S{S}'}$ 爲聲源的運動矢量， $\overrightarrow{D{D}'}$ 爲觀測者的運動矢量，這時從聲源 S 到觀測者 D 傳播方向的矢量 SD 隨時在變。設聲源在時刻t=t0 和t0+dt 的位置分別爲S 和 S′ ，相位分別爲 $\varphi$ 和 ${\varphi }'$ ，其中相位的增量爲 $d\varphi ={\varphi }'-\varphi =2\pi f_{t}dt$ 。在t0時刻，由聲源 S 發出的相位爲 $\varphi$ 的聲波傳播到 D,觀測者的位置在 D ；t0+dt 時刻，聲源到達 S′ ，由於觀測者也在運動，所以，聲源 S′ 發出的相位爲 $\varphi$ 的聲波只能到達觀測者的新位置D′，觀測者從D走到D′所用的時間dt′和他感受到的頻率 $f_{r}$ ，與聲源的和 $f_{t}$ 是不一樣的。在D接收的頻率 $f_{t}$ ，在D′接收的頻率 $f_{r}$ ，所接收聲波的相位變化了 $d\varphi =2\pi f_{r}dt^{'}$ ，由於相位增量是一樣的，即 $d\varphi =2\pi f_{t}dt=2\pi f_{r}dt^{'}$ 故

$f_{r}/f_{t} = dt/dt^{'}$ (1)

相位 $\varphi$ 從聲源 S 傳播到觀測者的位置 D 的時刻爲 $t=t_{0}+\left \| \vec{SD} \right \|/c$ ；而相位 $\varphi +d\varphi$ 由聲源 S′ 傳播到觀測者的位置 D′ 的時刻爲 $t=t_{0}+dt+\left \| \vec{{S}'{D}'} \right \|/c$ 。二者之差即爲

$d{t}'=dt+\left \| \vec{{S}'{D}'} \right \|/c-\left \| \vec{SD} \right \|/c$ (2)

如圖 1 所示，從、作SD的垂線，令相應的垂足分別爲 $S_{0}$ 、 $D_{0}$ 。由於 $S_{0}D_{0}$ 與的長度相差高階無窮小量，可認爲二者大小近似相等，於是

$\left \| \vec{SD} \right \|-\left \| \vec{{S}'{D}'} \right \|\approx \left \| \vec{SD} \right \|-\left \| \vec{S_{0}D_{0}} \right \|=\left \| \vec{SS_{0}} \right \|+\left \| \vec{DD_{0}} \right \|$

$=\left \| \vec{S{S}'} \right \|\cos \theta +\left \| \vec{D{D}'} \right \|\cos \psi$ (3)

式中，θ 是 $\vec{S{S}'}$ 與 $\vec{SD}$ 之間的夾角，ψ 則是 $\vec{S{D}'}$ 與 $\vec{DS}$ 之間的夾角。

由於聲波在水體介質中的傳播速度遠大於聲源和觀測者的運動速度，可認爲聲源和觀測者近似作勻速直線運動，因此 $\left \| \vec{S{S}'} \right \|=v_{t}dt$ ， $\left \| \vec{D{D}'} \right \|=v_{r}d{t}'$ 。由式(2)和式(2-3)解得

$d{t}'=dt-v_{t}dt\cos \theta /c-v_{r}d{t}'\cos \psi /c$ (4)

由此得到

$\frac{f_{r}}{f_{t}}=\frac{dt}{d{t}'}=\frac{c+v_{r}\cos \psi }{c-v_{t}\cos \theta }$ (5)

這便是多普勒效應的普遍公式。

1.2多普勒測速公式

不難看出，當θ 、ψ 都等於0 時，式(5)可以過渡到共線情形

$f_{r}=\frac{c+v_{r}}{c-v_{t}}f_{t}$ (6)

由式（6）可知，當聲源與觀測者相互靠近時， $f_{r}> f_{t}$ ；當聲源與觀測者相互遠離時， $f_{r}< f_{t}$ 。

對於DVL而言，收發合制換能器。換能器首先作爲聲源，水底作爲觀測者，於是根據是（6）水底接收的聲波頻率爲：

${f}'=\frac{c+v_{r}}{c-v_{t}}f_{t}$ (7)

之後水底作爲聲源，而換能器則作爲觀測者，於是換能器接收的聲波頻率爲：
$f_{r}=\frac{c+v_{t}}{c-v_{r}}{f}'$ (8)

整合式（7）與式（8），得到：

$f_{r}=\frac{(c+v_{r})(c+v_{t})}{(c-v_{t})(c-v_{r})}f_{t}$ (9)

換能器相對靜止、即當 $v_{t}=0$ 時， $v_{r}$ 就是水底相對換能器的徑向流速。於是，式(9)經過整理得到：

$f_{r}=\frac{c+v_{r}}{c-v_{r}}f_{t}=\frac{1+2v_{r}/c+(v_{r}/c)^{2}}{1-(v_{r}/c)^{2}}f_{t}$ (10)

由此經過整理可以得到通用的徑向測流基本公式

$v_{r}=\frac{f_{r}-f_{t}}{f_{r}+f_{t}}c=\frac{f_{d}}{2f_{t}+f_{d}}c$ (11)

其中 $f_{d}=f_{r}-f_{t}$ 爲多普勒頻移。由於水中聲速 c 約爲1500m/s，遠大於水體流速 $v_{r}$ ，於是一般可以忽略式(10)中 $v_{r}/c$ 的二次項，再經過整理就可以得到近似的多普勒徑向測流公式：

$v_{r}=\frac{c}{2f_{t}}(f_{r}-f_{t})=\frac{\lambda }{2}f_{d}$ (12)

式(2-12)中， $\lambda =c/f_{t}$ 爲換能器發射的信號波長， $f_{d}$ 爲收發信號之間的多普勒頻移。由此通過測量 $f_{d}$ 就可以得到換能器相對徑向水底速度 $v_{r}$ ，並可以進一步得到水平與垂直方向的速度。

2.DVL換能器陣型與座標變換

2.1Janus陣型結構

本節介紹多普勒測流常用的四波束Janus陣型結構。具有該換能器陣型結構的DVL可以同時發射和同時接收四個窄波束聲波，再經處理就可以得到相對於聲學換能器的徑向流速信息。在此定義DVL的右手座標系D 。D 的原點位於 Janus 陣型結構的等效中心O，它的三個軸 $x_{d}$ 、 $y_{d}$ 和 $z_{d}$ 分別指向測流系統所規定的前、右和下的方向。DVL的四波束Janus陣型結構是由四個換能器構成，各換能器收發聲波的軸線與測流系統座標系z軸之間的夾角爲α，如圖 2 所示。

圖2.四波束Janus陣型結構

另外，各換能器收發聲波的軸線在基陣座標系 xoy 平面的投影則形成兩條相互垂直的直線，且波束 1 的投影與x 軸之間的夾角爲 β，如圖3 所示。通常測流系統成“X”字形式安裝時的 β 等於 45°，成“十”字形式安裝時的 β 等於 0°。配有四波束 Janus 陣型結構的DVL在工作時，換能器同時發射的窄波束聲波，沿着各自換能器的軸線方向傳播。在傳播的過程中，每路聲波被該路水體散射，會有散射回波被各換能器接收。DVL換能器和水體之間的徑向運動使得收發聲波頻率之間存在多普勒頻移。通過測量多普勒頻移就可利用式(12)解算出徑向流速 $v_{1}$ ， $v_{2}$ ， $v_{3}$ 和 $v_{4}$ 。

圖3.四波束Janus陣型結構

2.2基陣座標系下的速度

設水底在D下的速度矢量爲 $\mathbf{V}_{d}=[v_{dx},v_{dy},v_{dz}]^{T}$ 。同時假設DVL所測水底是均勻水平的。配有四波束 Janus 陣型結構的DVL利用任意三個聲波所測流速就可以轉換爲D下的流速矢量 $\mathbf{V}_{d}$ 。即使因爲某一波束受到干擾不能正常測量時系統也可以工作，因此提高了測量的可靠性。以第 1、2 和 3 波束測得的流速列矢量 $\mathbf{V}_{b}=[v_{1},v_{2},v_{3}]^{T}$ 爲例，得到 $\mathbf{V}_{b}$ 與 $\mathbf{V}_{d}$ 的關係爲：

$\mathbf{V}_{b}=\mathbf{A}\mathbf{V}_{d}$ (13)

$\mathbf{V}_{d}=(\mathbf{A}^{T}\mathbf{A})^{-1}\mathbf{A}^{T}\mathbf{V}_{b}$ (14)

其中

$\mathbf{A}=\begin{bmatrix} \sin \alpha \cos \beta &\sin \alpha \sin \beta &\cos \alpha \\ -\sin \alpha \cos \beta&\sin \alpha \cos \beta &\cos \alpha \\ -\sin \alpha \cos \beta &-\sin \alpha \sin \beta & \cos \alpha \end{bmatrix}$ (15)

$\left ( \mathbf{A}^{T}\mathbf{A} \right )^{-1}\mathbf{A}^{T}=\begin{bmatrix} 1/\sin \alpha \cos \beta &-1/\sin \alpha \sin \beta &-1/\sin \alpha \cos \beta \\ 1/\sin \alpha \sin \beta&1/\sin \alpha \cos \beta &-1/\sin \alpha\sin \beta \\ 1/\cos \alpha &1/\cos \alpha & 1/\cos \alpha \end{bmatrix}$ (16)

實際上，所測水底很少是完全均勻的，即DVL在同一深度上所測相對水底的速度大小與方向往往不同。水底的不均勻性會引入不同程度的速度固有誤差。四波束 Janus 陣型結構較三波束 Convex 陣型結構可以多獲得第 4 波束的冗餘信息，因此可以驗證水底的均勻性情況。爲了量化水底不均勻性造成的影響，可以引入流速固有誤差公式，這是評估數據質量是否有效的重要因素。具體爲：

$v_{error}=v_{1}-v_{2}+v_{3}-v_{4}$ (17)

通過分析可知，只要所測水底均勻，則無論系統怎樣搖擺 $v_{error}$ 都基本趨近於零，且四路回波認爲是有效數據；如果所測流場不均勻，也要看 $v_{error}$ 來確定不均勻的程度，用於判斷四路回波數據的有效性。

四波束 Janus 陣型結構較三波束 Convex 陣型結構還有一個優勢，就是可以更有效地減小由DVL基陣搖擺引入的流速測量誤差，這是通過四個波束所測流速轉化爲D 下的流速矢量 $\mathbf{V}_{d}$ 來實現的。設水底的四個徑向速度構成的列矢量爲 $\mathbf{V_{b}}=[v_{1},v_{2},v_{3},v_{4}]^{T}$ ，由於水底在D下的流速列矢量爲 $\mathbf{V_{d}}=[v_{dx},v_{dy},v_{dz}]^{T}$ ，於是得到 $\mathbf{V_{b}}$ 與 $\mathbf{V_{d}}$ 之間的關係爲：

$\mathbf{V_{b}}=\mathbf{BV_{d}}$ (18)

$\mathbf{V_{d}}=(\mathbf{B^{T}B})^{-1}\mathbf{B^{T}V_{b}}$ (19)

式（18）和式（19）中的轉換矩陣分別爲：

$\mathbf{B}=\left ( \begin{matrix} \sin \alpha \cos \beta &\sin \alpha \sin \beta & \cos \alpha \\ -\sin \alpha \sin \beta &\sin \alpha \cos\beta & \cos \alpha\\ -\sin \alpha \cos \beta &-\sin \alpha \sin \beta & \cos \alpha\\ \sin \alpha \sin \beta &-\sin \alpha \cos\beta & \cos \alpha \end{matrix} \right )$ (20)

$\mathbf{(B^{T}B)^{-1}B^{T}}=\left ( \begin{matrix} 1/\sin\alpha\cos\beta &-1/\sin\alpha\sin\beta &-1/\sin\alpha\cos\beta &1/\sin\alpha\sin\beta \\ 1/\sin\alpha\sin\beta &1/\sin\alpha\cos\beta &-1/\sin\alpha\sin\beta &-1/\sin\alpha\cos\beta \\ 1/\cos \alpha & 1/\cos \alpha & 1/\cos \alpha & 1/\cos \alpha \end{matrix} \right )$ (21)

由式（14）和式（19）的關係，最後就可以將測得的徑向速度 $\mathbf{V_{b}}$ 轉換爲測速系統座標系D下的速度 $\mathbf{V_{d}}$ 。

2.3大地座標系下的速度

相對於聲學換能器的徑向流速和基陣座標系下的流速往往並不是用戶想要得到的最終流速信息。在實際應用過程中，大地座標系下的流速信息往往具有更好的實用價值，因此需要將所測換能器徑向流速轉換成基陣座標系下的流速後，再進一步轉換爲大地座標系下的流速，而這就涉及到了兩個座標系下的流速座標轉換問題。

圖4. 座標轉換

在此定義大地的右手座標系G。大地座標系G的原點位於測速系統的等效中心O，它的三個軸 $\mathbf{x}_{g}$ 、 $\mathbf{y}_{g}$ 和 $\mathbf{z}_{g}$ 分別指向北、東和垂直向下。接下來，要將基陣座標系D下的速度轉換爲大地座標系G下的速度。如圖4 所示設某一右手直角座標系xyz就是基陣座標系D，首先繞oz軸順時針旋轉一個角度ψ，則相應的方向餘弦矩陣 $\mathbf{\Psi}$ 爲：

$\mathbf{\Psi }=\left ( \begin{matrix} \cos \psi & \sin \psi &0 \\ -\sin \psi & \cos \psi &0 \\ 0 & 0 & 1 \end{matrix} \right )$ （22）

接下來繞oy軸順時針旋轉一個角度θ，則相應的方向餘弦矩陣 $\mathbf{\Theta }$ 爲：

$\mathbf{\Theta} =\left ( \begin{matrix} \cos \theta & 0 &-\sin \theta \\ 0 & 1 & 0\\ \sin \theta & 0 & \cos \theta \end{matrix} \right )$ （23）

最後繞ox 軸順時針旋轉一個角度 $\varphi$ ，則相應的方向餘弦矩陣 $\mathbf{\Phi }$ 爲：

$\mathbf{\Phi }=\left ( \begin{matrix} 1 & 0 &0 \\ 0 & \cos \varphi & \sin \varphi \\ 0 & -\sin \varphi & \cos \varphi \end{matrix} \right )$ (24)

此時ψ、θ與 $\varphi$ 爲三個歐拉角，他們分別稱爲航向角、縱搖角和橫搖角，這是通過DVL內嵌的姿態傳感器提供的。

設經過以上三次旋轉後的新右手直角座標系 $\mathbf{{x}'{y}'{z}'}$ 爲大地座標系G，則 $\mathbf{xyz}$ 到 $\mathbf{{x}'{y}'{z}'}$ 的座標轉換矩陣 $\mathbf{R}$ 爲：

$\mathbf{R}=\mathbf{\Psi \Theta \Phi }$ (25)

設速度矢量在D內座標爲 $\mathbf{V_{d}}$ ，在G內座標爲 $\mathbf{V_{g}}$ ，則有如下關係成立：

$\mathbf{V_{g}}=\mathbf{RV_{d}}$ (26)

$\mathbf{V_{d}}=\mathbf{R^{-1}V_{g}}=\mathbf{R^{T}V_{g}}$ (27)

由式（26）的關係，就可以將基陣座標系D下的流速轉換爲大地座標系G下的流速。最後由式(18)、(19)與式(26)、(27)，得到 $\mathbf{V_{b}}$ 與 $\mathbf{V_{g}}$ 的通用關係：

$\mathbf{V_{g}}=\mathbf{R(B^{T}B)^{-1}B^{T}V_{b}}$ (28)

$\mathbf{V_{b}}=\mathbf{BR^{-1}V_{g}}=\mathbf{BR^{T}V_{g}}$ (29)

由式（28）與式（29），就可以將波束所測徑向速度直接轉換爲大地座標系下的速度。

3.DVL的信號處理

3.1頻率估計算法簡述

如何從多普勒測速回波中提取流速信息是一個重要的問題。該問題涉及到一定空間範圍內平均多普勒頻移的測量。目前，常用的解決方法是將該問題簡化爲單頻復正弦信號加上高斯白噪聲的頻率估計問題。

基於此觀點，有很多可以利用的算法來估計頻率。其中大部分是基於最大似然(ML)的方法。實際的 ML 估計器需要定位週期圖頻譜譜峯的位置，其中，Palmer 利用了 FFT 來估計譜峯位置，但性能並不理想。Rife 和 Boorstyn給出了最大似然估計器的公式，此時的該估計是無偏的且在信噪比(SNR)達到門限值以上時該估計的均方誤差可以達到克拉美-羅下限(CLRB)。需要注意，以上基於頻域的估計算法計算時較爲複雜，目前並不適合DVL快速靈活的測量要求。

減小頻域估計器計算量常用的方法是基於時域相位的估計思想。通過將加性高斯噪聲近似爲高斯相位噪聲，Tretter給出了一種時域估計器。該近似在高 SNR 條件下是有效的，且作爲一種最小二乘估計器其性能等價於 ML的性能。由於需要解決相位模糊問題，該估計器在低 SNR 時性能很難滿足。Kay給出了一個延遲採樣間隔的相位差分估計器，該估計器在高 SNR 條件下可以達到 CRLB。由於改變了提取相位與求和運算的順序，該估計器可以被認爲是由原來的一種相位平均估計器(PA)轉變爲一種線性預測估計器(LP)。以 Kay 的工作爲基礎，不同延遲和不同權重的時域頻率估計器相繼出現。這些估計器在性能上比 Kay 的估計器有所提高，但這是以限制頻率估計範圍或增加計算量爲前提的。Brown 和 Wang，肖揚燦分別給出了各自的循環頻率估計器。但這些循環形式的頻率估計器性能的提高也是以進一步增加計算複雜性爲代價的。

還有其它頻率估計的典型算法，包括過零檢測法，自適應法等。過零檢測法是在一個過零點處開始以非常高的時鐘脈衝計數，來確定 N 個信號週期所需的時間，從而進一步估計頻率。該算法的運算量小且實現邏輯簡單，但當信噪比較低時精度不能令人滿意。自適應法是用最小均方算法(LMS)自適應調整基於自迴歸 AR 模型的 LP 的係數，然後通過對頻率軸的掃描根據 LP譜峯值來確定信號的頻率。自適應法可以實現高精度和連續調節的窄帶頻率估計，但該算法需要一定的自適應時間。由過零檢測法和自適應法的以上特點可以看出，它們無法應用到寬帶回波的多普勒頻移估計中。

由波束散射模型可以看出，任意時刻的回波都是全部發射信號在對應某空間範圍內的響應。這一情況使得回波與發射信號之間差異很大，特別是在發射信號形式較爲複雜的時候。由於信號形式的不同情況，選擇流速估計器的主要原則是在保證一定測速性能的基礎上，考慮快速靈活的測量要求。可以看到，基於時域相位思想的復自相關算法因其快速靈活可控的特點而成爲了流速估計的一個合適選擇。

3.2復自相關算法

復自相關算法的主要思想是確定兩段回波信號之間的幅值和相位關係，從而確定兩段回波信號之間的頻率。復自相關算法適合於不同信號下的流速的測量，因而是應用較爲廣泛的方法。從功率譜的觀點出發，多普勒頻移測量的問題就轉化爲確定觀測信號功率譜密度的一階矩。在此令回波的複數觀測信號爲：

(30)

這裏s(t)是含有多普勒信息的複數觀測信號，n(t)爲零均值獨立平穩復高斯噪聲。x(t) 對應的協方差函數與功率譜密度可以分別表示爲： $R(\tau )=R_{s}(\tau )+R_{n}(\tau ),S(f)=S_{s}(f)+S_{n}(f)$ 。於是主要問題就是估計多普勒頻率均值 $\mu_{s} (f)$ ，它可以用s(t)功率譜的一階矩表示爲

$\mu_{s} (f)=\frac{\int_{-\infty }^{\infty }fS_{s}(f)df}{\int_{-\infty }^{\infty }S_{s}(f)df}$ (31)

首先，將 x(t),s(t)與n(t)的自相關函數表示成極座標形式

$R(\tau )=\left | R(\tau ) \right |\exp(j\phi (\tau ))$ (32)

$R_{s}(\tau )=\left | R_{s}(\tau ) \right |\exp(j\phi_{s} (\tau )),R_{n}(\tau )=\left | R_{n}(\tau ) \right |\exp(j\phi_{n} (\tau ))$ (33)

由於假定噪聲是非零獨立平穩高斯的，所以有

$R_{n}(\tau )=\left\{\begin{matrix} R_{n}(0), & \tau =0\\ 0, & \tau \neq 0 \end{matrix}\right.$ (34)

因此就有 $R(\tau )=R_{s}(\tau ),\tau \neq 0$ 。這說明 $\tau \neq 0$ 時對 $R(\tau )$ 的計算可得到 $R_{s}(\tau )$ ，即可以用 $R(\tau )$ 來代替 $R_{s}(\tau )$ 。

其次，對式（33）的 $R_{s}(\tau )$ 求導，有

${R_{s}}'(\tau )=\frac{dR_{s}(\tau )}{d\tau }=[{\left |R_{s}(\tau ) \right |}'+j\left | R_{s}(\tau ) \right |{\phi _{s}}'(\tau )]\exp (j\phi _{s}(\tau ))$ (35)

由於 $\left | R_{s}(\tau ) \right |$ 爲偶函數， ${\left | R_{s}(\tau ) \right |}'$ 爲奇函數，所以 ${\left | R_{s}(0 ) \right |}'=0$ 。因此

${R_{s}}'(0)=jR_{s}(0){\phi _{s}}'(0)$ (36)

另外根據維納-辛欽定理

$R_{s}(\tau )=\int_{-\infty }^{\infty }S_{s}(f)\exp (j2\pi \tau f)df$ (37)

$S_{s}(f)=\int_{-\infty }^{\infty }S_{s}(\tau)\exp (-j2\pi f\tau)d\tau$ (38)

對式(37)的 $R_{s}(\tau )$ 求導，有

${R_{s}}'(\tau )=j2\pi \int_{-\infty }^{\infty }fS_{s}(f)\exp (j2\pi \tau f)df$ (39)

當 $\tau =0$ 時，式（37）與式（39）式變爲

${R_{s}}'(0)=j2\pi \int_{-\infty }^{\infty }fS_{s}(f)df$ (40)

$R_{s}(0)=\int_{-\infty }^{\infty }S_{s}(f)df$ (41)

最後，將式（36）、式（40）與式（41）帶入到式（31），得到

$\mu_{s} (f)=\frac{\int_{-\infty }^{\infty }fS_{s}(f)df}{\int_{-\infty }^{\infty }S_{s}(f)df}=\frac{-j{R_{s}}'(0)}{2\pi R_{s}(0)}=\frac{1}{2\pi }{\phi _{s}}'(0)$ (42)

由式（42）可以看出，多普勒頻率均值 $\mu_{s} (f)$ 可以由回波複數觀測信號自相關函數相位的導數來表示。於是進一步可以看出，對於線性相位情況，就有如下關係成立

$\mu_{s} (f)=\frac{\phi (\tau )}{2\pi \tau }$ (43)

式(43)就是得到的多普勒頻率均值 $\mu_{s} (f)$ 的復自相關算法的估計公式，其中 $\phi (\tau )$ 是自相關函數的相位。由復自相關函數及其相位的公式

$R(\tau )=\frac{1}{T-\left | \tau \right |}\int_{0}^{T-\left | \tau \right |}x(t+\tau )x^{*}(t)dt,\phi (\tau )=\arctan \frac{Im{R(\tau )}}{Re{R(\tau )}}$ (44)

對於複數觀測信號 $x(t)=x_{r}(t)+jx_{i}(t),\phi (\tau )$ 可以表示爲

$\phi(\tau )=\arctan \frac{\int_{0}^{T-\left | \tau \right |}x_{r}(t)x_{i}(t+\tau )-x_{i}(t)x_{r}(t+\tau )dt}{\int_{0}^{T-\left | \tau \right |}x_{r}(t)x_{r}(t+\tau )+x_{i}(t)x_{i}(t+\tau )dt}$ (45)

以上的式(45)等價於 Kay 所給出的線性預測估計器。值得注意的是，由於反正切運算使得相位函數被限制在[−π,π]之間，而真實 $\phi (\tau )$ 的取值不應該被限制在這一範圍。

由復自相關算法的估計公式可以看出，不論是發射單載頻矩形脈衝信號，還是發射其它更爲複雜的信號形式，只要具有時間間隔的兩段回波信號之間存在着相干性，就都可以通過復自相關算法確定這兩段回波信號之間的多普勒頻率。因此基於復自相關算法的回波處理，多普勒速度測量將會變得非常靈活。

4.DVL系統仿真分析

4.1DVL系統CW回波信號的生成

根據前面的理論分析，下面給出的是基於CW信號的回波生成函數的M代碼：

function Orignal = DVL_CW_echo_gen(f0,fs,tao,c,total_time,SNR,H,vg,attitude_angle,alpha,beta)
% ************************** DVL CW信號生成回波信號函數 **************************
% 假設海底是一個平面 
% 依據Janus陣型結構四子陣測得的徑向多普勒速度,根據轉換矩陣B轉換得到基陣座標系下
% 的速度,然後再依據轉換矩陣R得到大地座標系下的速度.
% 參考文獻：[1] 劉德鑄,聲學多普勒流速測量關鍵技術研究,2.4節.
%                    ∧
%           子陣1    ||y    子陣2
%                    ||
% 行船方向 <-----x---||---------
%                    ||
%           子陣4    ||     子陣3
%                    ||
% **** 輸入參數 ****
% **   f0                  信號頻率                      單位：Hz
% **   fs                  採樣率                        單位：Hz
% **   tao                 發射信號脈寬                   單位：s
% **   c                   聲速                          單位：m/s
% **   total_time          採樣信號總時間                 單位：s
% **   SNR                 信噪比                         單位：dB
% **   H                   DVL距離海底的垂直高度                                 單位：m
% **   vg                  DVL相對於大地座標系的三維速度，各分量分別位Vx,Vy,Vz    單位：m/s
% **   attitude_angle      DVL的姿態角,各分量分別爲Roll,Pitch,Yaw                單位：°
% **   alpha               DVL換能器軸線方向與換能器陣軸線方向夾角，即安裝角，     單位：°
% **   beta                X型Janus陣型結構逆時針與座標軸的夾角                   單位：°
% **** 輸出參數 ****
% **   Orignal             各通道的原始數據      
%%
% 相關參數計算
alpha = alpha*pi./180;% 與z軸的夾角
beta = beta*pi./180;% X型Janus陣型結構逆時針與座標軸的夾角
B = [sin(alpha)*cos(beta),sin(alpha)*sin(beta),cos(alpha);
    -sin(alpha)*sin(beta),sin(alpha)*cos(beta),cos(alpha);
    -sin(alpha)*cos(beta),-sin(alpha)*sin(beta),cos(alpha);
    sin(alpha)*sin(beta),-sin(alpha)*cos(beta),cos(alpha)];

fai =   attitude_angle(1)*pi./180;% 橫搖 roll 沿x軸順時針旋轉
theta = attitude_angle(2)*pi./180;% 縱搖 pitch 沿y軸順時針旋轉
pfai =  attitude_angle(3)*pi./180;% 艏搖 yaw 偏航 沿z軸順時針旋轉

Fai = [1,0,0;
       0,cos(fai),sin(fai);
       0,-sin(fai),cos(fai)];
Theta = [cos(theta),0,-sin(theta);
        0,1,0;
        sin(theta),0,cos(theta)];
Pfai = [cos(pfai),sin(pfai),0;
        -sin(pfai),cos(pfai),0;
        0,0,1];
R = Pfai*Theta*Fai;% 座標轉換矩陣

% vd = pinv(B)*vb;% 基陣座標系下的速度
% vg = R*pinv(B)*vb;% 大地座標系下的速度
vb = B*pinv(R)*vg;  % 由大地座標系下的速度得到換能器的多普勒徑向速度 

DIRO1 = [sin(alpha)*cos(beta);sin(alpha)*sin(beta);cos(alpha)];    %在換能器基陣座標系中，1號換能器方向上的單位向量
DIRO2 = [-sin(alpha)*cos(beta);sin(alpha)*sin(beta);cos(alpha)];   %在換能器基陣座標系中，2號換能器方向上的單位向量
DIRO3 = [-sin(alpha)*cos(beta);-sin(alpha)*sin(beta);cos(alpha)];  %在換能器基陣座標系中，3號換能器方向上的單位向量
DIRO4 = [sin(alpha)*cos(beta);-sin(alpha)*sin(beta);cos(alpha)];   %在換能器基陣座標系中，4號換能器方向上的單位向量

DIRO1_g = R*DIRO1;            %在大地座標系中，1號換能器方向上的單位向量
DIRO2_g = R*DIRO2;            %在大地座標系中，2號換能器方向上的單位向量
DIRO3_g = R*DIRO3;            %在大地座標系中，3號換能器方向上的單位向量
DIRO4_g = R*DIRO4;            %在大地座標系中，4號換能器方向上的單位向量
DIRZ_g = [0;0;1];             %大地座標系中Z軸方向的單位向量

dis1 = H/(DIRO1_g'*DIRZ_g);   %1號換能器的聲波傳播距離
dis2 = H/(DIRO2_g'*DIRZ_g);   %2號換能器的聲波傳播距離
dis3 = H/(DIRO3_g'*DIRZ_g);   %3號換能器的聲波傳播距離
dis4 = H/(DIRO4_g'*DIRZ_g);   %4號換能器的聲波傳播距離
dis = [dis1;dis2;dis3;dis4];  
%%
% 生成各基元回波信號
sig_points = fix(total_time*fs);  %總採樣點數
Orignal = zeros(4,sig_points);
temp = 10^(-SNR/20);

for i = 1:4
    tt = 2*dis(i)/c;             %計算回波的開始時刻 
    d_f = f0*2*vb(i)/c;          %計算多普勒頻移
    f_r = f0 + d_f;              %接收的回波信號頻率
    tao_compressd = tao/(1+2*vb(i)/c);     %計算壓縮後的脈寬
    nTs = 0:1/fs:tao_compressd;            %被壓縮的回波信號時刻
    Echo_points = length(nTs);             %被壓縮的回波採樣點數
    
    tt_num = fix(tt*fs)+1;                   %轉換成時刻點
    phase = (tt_num/fs - tt)*f_r*2*pi;       %採樣認爲回波信號開始的時刻與真實的開始時刻相位差  
    Orignal(i,tt_num:(tt_num + Echo_points-1)) = cos(2*pi*f_r*nTs + phase);     %對回波信號採樣

    noise = randn(1,sig_points);
    noise = noise * temp;
    Orignal(i,:) = Orignal(i,:) + noise;            %加入一定信噪比的噪聲
end

4.2基於仿真信號的DVL系統仿真

由前面的理論分析，採用復自相關算法進行信號處理，編寫的M代碼如下：

% DVL信號處理
close all
clear all
clc

c = 1500;
fs = 600e3;                %採樣率
f0 = 400e3;                %CW信號頻率
tao = 5.0e-3;
cw_with_ms = tao*1000;
total_time = 0.14;
H = 10;                                 %深度
alpha = 22.5;                    %換能器軸線方向與換能器陣軸線方向夾角，即安裝角，單位：°
beta = 45;                       %X型Janus陣型結構逆時針與座標軸的夾角           單位：°
attitude_angle = [2 5 4.5];      %姿態角，此時基陣座標與大地座標重合
vg1 = [1;-1;0.5];                   %DVL相對於大地座標系速度
SNR = 40;                        %信噪比

Orignal1 = DVL_CW_echo_gen(f0,fs,tao,c,total_time,SNR,H,vg1,attitude_angle,alpha,beta);

discard_points = 1;                   %捨去一開始的點數
end_points = 30000;
ORI_Channel1 = Orignal1(1,discard_points:end_points);      %讀取第一個通道的數據
ORI_Channel2 = Orignal1(2,discard_points:end_points);
ORI_Channel3 = Orignal1(3,discard_points:end_points);
ORI_Channel4 = Orignal1(4,discard_points:end_points);

sample_points = length(ORI_Channel1);   %獲取一個通道的採樣點數
nTs = (0:sample_points-1)/fs + cw_with_ms/1000 + discard_points/fs;  %採樣時刻
sig_points = fix(cw_with_ms*fs/1000*1)+1; %信號脈寬一半佔的採樣點數/2
delay_n = 1;            %延時點數fix(sig_points/2)/2  fix(sig_points*1/2)

figure;
plot(ORI_Channel1,'r');
hold on;
grid on;
plot(ORI_Channel2,'b');
plot(ORI_Channel3,'c');
plot(ORI_Channel4,'g');
legend('通道1','通道2','通道3','通道4');
title('原始數據');
figure;
plot(nTs,ORI_Channel1,'r');
hold on;
grid on;
plot(nTs,ORI_Channel2,'b');
plot(nTs,ORI_Channel3,'c');
plot(nTs,ORI_Channel4,'g');
legend('通道1','通道2','通道3','通道4');
xlabel('時間/s');
title('原始數據');
%%
%正交變換
cos_table = cos(2*pi*f0*nTs);
sin_table = sin(2*pi*f0*nTs);

cos_ORI_Channel1 = ORI_Channel1.*cos(2*pi*f0*nTs);
sin_ORI_Channel1 = ORI_Channel1.*sin(2*pi*f0*nTs);
cos_ORI_Channel2 = ORI_Channel2.*cos(2*pi*f0*nTs);
sin_ORI_Channel2 = ORI_Channel2.*sin(2*pi*f0*nTs);
cos_ORI_Channel3 = ORI_Channel3.*cos(2*pi*f0*nTs);
sin_ORI_Channel3 = ORI_Channel3.*sin(2*pi*f0*nTs);
cos_ORI_Channel4 = ORI_Channel4.*cos(2*pi*f0*nTs);
sin_ORI_Channel4 = ORI_Channel4.*sin(2*pi*f0*nTs);

figure;
subplot(2,1,1);
plot(cos_ORI_Channel1,'r');
hold on;
grid on;
plot(cos_ORI_Channel2,'b');
plot(cos_ORI_Channel3,'c');
plot(cos_ORI_Channel4,'g');
legend('通道1','通道2','通道3','通道4');
title('*cos');
subplot(2,1,2);
plot(sin_ORI_Channel1,'r');
hold on;
grid on;
plot(sin_ORI_Channel2,'b');
plot(sin_ORI_Channel3,'c');
plot(sin_ORI_Channel4,'g');
legend('通道1','通道2','通道3','通道4');
title('*sin');
%*************** LPF Fpass = 16KHz  Fstop = 40kHz 64階 *************************
hn = [0.0004    0.0004    0.0004    0.0002   -0.0001   -0.0007   -0.0014   -0.0020   -0.0023   -0.0021   -0.0012    0.0006    0.0029    0.0053...
      0.0073    0.0081    0.0072    0.0040   -0.0013   -0.0080   -0.0151   -0.0209   -0.0236   -0.0213   -0.0130    0.0019    0.0228    0.0480...
      0.0749    0.1006    0.1218    0.1358    0.1407    0.1358    0.1218    0.1006    0.0749    0.0480    0.0228    0.0019   -0.0130   -0.0213...
      -0.0236   -0.0209   -0.0151   -0.0080   -0.0013    0.0040    0.0072    0.0081    0.0073    0.0053    0.0029    0.0006   -0.0012   -0.0021...
      -0.0023   -0.0020   -0.0014   -0.0007   -0.0001    0.0002    0.0004    0.0004    0.0004];

LPF_cos_ORI_Channel1 = filter(hn,1,cos_ORI_Channel1);
LPF_sin_ORI_Channel1 = filter(hn,1,sin_ORI_Channel1);
LPF_cos_ORI_Channel2 = filter(hn,1,cos_ORI_Channel2);
LPF_sin_ORI_Channel2 = filter(hn,1,sin_ORI_Channel2);
LPF_cos_ORI_Channel3 = filter(hn,1,cos_ORI_Channel3);
LPF_sin_ORI_Channel3 = filter(hn,1,sin_ORI_Channel3);
LPF_cos_ORI_Channel4 = filter(hn,1,cos_ORI_Channel4);
LPF_sin_ORI_Channel4 = filter(hn,1,sin_ORI_Channel4);

figure;
subplot(2,1,1);
plot(LPF_cos_ORI_Channel1,'r');
hold on;
grid on;
plot(LPF_cos_ORI_Channel2,'b');
plot(LPF_cos_ORI_Channel3,'c');
plot(LPF_cos_ORI_Channel4,'g');
legend('通道1','通道2','通道3','通道4');
title('*cos經過LPF');
subplot(2,1,2);
plot(LPF_sin_ORI_Channel1,'r');
hold on;
grid on;
plot(LPF_sin_ORI_Channel2,'b');
plot(LPF_sin_ORI_Channel3,'c');
plot(LPF_sin_ORI_Channel4,'g');
legend('通道1','通道2','通道3','通道4');
title('*sin經過LPF');

%% 
%生成複數
complex_ORI_Channel1 = LPF_cos_ORI_Channel1 + 1j*LPF_sin_ORI_Channel1;
complex_ORI_Channel2 = LPF_cos_ORI_Channel2 + 1j*LPF_sin_ORI_Channel2;
complex_ORI_Channel3 = LPF_cos_ORI_Channel3 + 1j*LPF_sin_ORI_Channel3;
complex_ORI_Channel4 = LPF_cos_ORI_Channel4 + 1j*LPF_sin_ORI_Channel4;

figure;
subplot(2,1,1);
plot(abs(complex_ORI_Channel1),'r');
hold on;
grid on;
plot(abs(complex_ORI_Channel2),'b');
plot(abs(complex_ORI_Channel3),'c');
plot(abs(complex_ORI_Channel4),'g');
legend('通道1','通道2','通道3','通道4');
title('覆信號幅度');
subplot(2,1,2);
plot(angle(complex_ORI_Channel1)/pi*180,'r');
hold on;
grid on;
plot(angle(complex_ORI_Channel2)/pi*180,'b');
plot(angle(complex_ORI_Channel3)/pi*180,'c');
plot(angle(complex_ORI_Channel4)/pi*180,'g');
legend('通道1','通道2','通道3','通道4');
title('相位°');

%%
% 複數自相關
autocor_points = sig_points - delay_n;
for i = (autocor_points+delay_n):sample_points
    set = (1:autocor_points) + i - (autocor_points);
    R_complex_ORI_Channel1(i) = complex_ORI_Channel1(set-delay_n)*complex_ORI_Channel1(set)';
    R_complex_ORI_Channel2(i) = complex_ORI_Channel2(set-delay_n)*complex_ORI_Channel2(set)';
    R_complex_ORI_Channel3(i) = complex_ORI_Channel3(set-delay_n)*complex_ORI_Channel3(set)';
    R_complex_ORI_Channel4(i) = complex_ORI_Channel4(set-delay_n)*complex_ORI_Channel4(set)';
end

Amp1 = abs(R_complex_ORI_Channel1);
Fai1 = angle(R_complex_ORI_Channel1);
Amp2 = abs(R_complex_ORI_Channel2);
Fai2 = angle(R_complex_ORI_Channel2);
Amp3 = abs(R_complex_ORI_Channel3);
Fai3 = angle(R_complex_ORI_Channel3);
Amp4 = abs(R_complex_ORI_Channel4);
Fai4 = angle(R_complex_ORI_Channel4);

figure;
plot(Fai1/pi*180,'r');
hold on;
grid on;
plot(Fai2/pi*180,'b');
plot(Fai3/pi*180,'c');
plot(Fai4/pi*180,'g');
legend('通道1','通道2','通道3','通道4');
title('復自相關相位');

%%

norm_amp1 = Amp1/max(Amp1);
norm_amp2 = Amp2/max(Amp2);
norm_amp3 = Amp3/max(Amp3);
norm_amp4 = Amp4/max(Amp4);

[echo_time1,v1,Fai_set1] = Echo_time_v_estimate(Amp1,sig_points,sample_points,Fai1,c,fs,f0,delay_n);
[echo_time2,v2,Fai_set2] = Echo_time_v_estimate(Amp2,sig_points,sample_points,Fai2,c,fs,f0,delay_n);
[echo_time3,v3,Fai_set3] = Echo_time_v_estimate(Amp3,sig_points,sample_points,Fai3,c,fs,f0,delay_n);
[echo_time4,v4,Fai_set4] = Echo_time_v_estimate(Amp4,sig_points,sample_points,Fai4,c,fs,f0,delay_n);

Echo_time = [echo_time1,echo_time2,echo_time3,echo_time4];
Echo_time = Echo_time/fs - tao;
H1 = DVL_Hight_Estimate(Echo_time,c,attitude_angle,alpha,beta);

figure;
plot(norm_amp1);
hold on;
grid on;
plot(Fai1/max(abs(Fai1)),'r');
plot(echo_time1,0,'*k');
plot(Fai_set1,Fai1(Fai_set1)/max(abs(Fai1)),'g*');
legend('複數自相關幅度','相位','估計的到達時刻','截取的相位區間');
string = ['估計的到達時刻 n=',num2str(echo_time1),',估計的速度 v1=',num2str(v1),'m/s'];
title(string);

figure;
plot(norm_amp2);
hold on;
grid on;
plot(Fai2/max(abs(Fai2)),'r');
plot(echo_time2,0,'*k');
plot(Fai_set2,Fai2(Fai_set2)/max(abs(Fai2)),'g*');
legend('複數自相關幅度','相位','估計的到達時刻','截取的相位區間');
string = ['估計的到達時刻 n=',num2str(echo_time2),',估計的速度 v2=',num2str(v2),'m/s'];
title(string);

figure;
plot(norm_amp3);
hold on;
grid on;
plot(Fai3/max(abs(Fai3)),'r');
plot(echo_time3,0,'*k');
plot(Fai_set3,Fai3(Fai_set3)/max(abs(Fai3)),'g*');
legend('複數自相關幅度','相位','估計的到達時刻','截取的相位區間');
string = ['估計的到達時刻 n=',num2str(echo_time3),',估計的速度 v3=',num2str(v3),'m/s'];
title(string);

figure;
plot(norm_amp4);
hold on;
grid on;
plot(Fai4/max(abs(Fai4)),'r');
plot(echo_time4,0,'*k');
plot(Fai_set4,Fai4(Fai_set4)/max(abs(Fai4)),'g*');
legend('複數自相關幅度','相位','估計的到達時刻','截取的相位區間');
string = ['估計的到達時刻 n=',num2str(echo_time4),',估計的速度 v4=',num2str(v4),'m/s'];
title(string);

vb = [v1 v2 v3 v4]';
[vg,vd] = Get_absolute_velocity(alpha,beta,vb,attitude_angle);   %相對大地速度，相對基陣速度

其中調用的三個子函數（Echo_time_v_estimate、DVL_Hight_Estimate、Get_absolute_velocity）代碼分別如下：

function [echo_time,v,Fai_set] = Echo_time_v_estimate(Amp,sig_points,sample_points,Fai,c,fs,f0,delay_n)
%****************************** 回波時間與速度估計 *************************
% **** 輸入參數
% **   Amp             覆信號自相關後的幅度
% **   sig_points      發射信號脈寬佔的採樣信號點數
% **   sample_points   總採樣點數，即信號總長度
% **   Fai             覆信號自相關後的相位
% **   c               聲速
% **   fs              採樣率
% **   f0              載波頻率
% **   delay_n         復自相關延遲的採樣點數
% **** 輸出參數
% **   echo_time       估計的回波時間
% **   v               估計的回波速度
% **   Fai_set         回波時刻對應的相位區間
%%
max_sum = sum(Amp(1:sig_points));
max_set = 1;
for i = 2:sample_points - sig_points
    temp = sum(Amp(i:(i+sig_points-1)));
    if temp > max_sum
        max_sum = temp;    %尋找最大值
        max_set = i;       %記錄最大值位置
    end
end

X = max_set:(max_set+sig_points-1);
Y = Amp(X);
echo_time = sum(X.*Y)/sum(Y);      %質量中心法求最大值點
echo_time_int = fix(echo_time);

Q_sig_points = fix(sig_points/4);
Fai_set = (echo_time_int-Q_sig_points):(echo_time_int+Q_sig_points);
Fai_avr = sum(Fai(Fai_set))/((2*Q_sig_points+1));

v = Fai_avr*c*fs/(4*pi*delay_n*f0);    %估計速度

function H = DVL_Hight_Estimate(Echo_time,c,attitude_angle,alpha,beta)
% *************** DVL高度估計 **********************
% 依據Janus陣型結構四子陣測得的徑向多普勒速度,根據轉換矩陣B轉換得到基陣座標系下
% 的速度,然後再依據轉換矩陣R得到大地座標系下的速度.
% 參考文獻：[1] 劉德鑄,聲學多普勒流速測量關鍵技術研究,2.4節.
%                    ∧
%           子陣1    ||y    子陣2
%                    ||
% 行船方向 <-----x---||---------
%                    ||
%           子陣4    ||     子陣3
%                    ||
% **** 輸入參數 ****
% **   Echo_time           由各換能器估計的到達時刻       單位：s
% **   c                   聲速                          單位：m/s
% **   attitude_angle      DVL的姿態角,各分量分別爲Roll,Pitch,Yaw                單位：°
% **   alpha               DVL換能器軸線方向與換能器陣軸線方向夾角，即安裝角，     單位：°
% **   beta                X型Janus陣型結構逆時針與座標軸的夾角                   單位：°
% **** 輸出參數 ****
% **   H                   估計的高度                     單位：m
%%
alpha = alpha*pi./180;% 與z軸的夾角
beta = beta*pi./180;% X型Janus陣型結構逆時針與座標軸的夾角
fai =   attitude_angle(1)*pi./180;% 橫搖 roll 沿x軸順時針旋轉
theta = attitude_angle(2)*pi./180;% 縱搖 pitch 沿y軸順時針旋轉
pfai =  attitude_angle(3)*pi./180;% 艏搖 yaw 偏航 沿z軸順時針旋轉

Fai = [1,0,0;
       0,cos(fai),sin(fai);
       0,-sin(fai),cos(fai)];
Theta = [cos(theta),0,-sin(theta);
        0,1,0;
        sin(theta),0,cos(theta)];
Pfai = [cos(pfai),sin(pfai),0;
        -sin(pfai),cos(pfai),0;
        0,0,1];
R = Pfai*Theta*Fai;% 座標轉換矩陣

DIRO1 = [sin(alpha)*cos(beta);sin(alpha)*sin(beta);cos(alpha)];    %在換能器基陣座標系中，1號換能器方向上的單位向量
DIRO2 = [-sin(alpha)*cos(beta);sin(alpha)*sin(beta);cos(alpha)];   %在換能器基陣座標系中，2號換能器方向上的單位向量
DIRO3 = [-sin(alpha)*cos(beta);-sin(alpha)*sin(beta);cos(alpha)];  %在換能器基陣座標系中，3號換能器方向上的單位向量
DIRO4 = [sin(alpha)*cos(beta);-sin(alpha)*sin(beta);cos(alpha)];   %在換能器基陣座標系中，4號換能器方向上的單位向量

DIRO1_g = R*DIRO1;            %在大地座標系中，1號換能器方向上的單位向量
DIRO2_g = R*DIRO2;            %在大地座標系中，2號換能器方向上的單位向量
DIRO3_g = R*DIRO3;            %在大地座標系中，3號換能器方向上的單位向量
DIRO4_g = R*DIRO4;            %在大地座標系中，4號換能器方向上的單位向量
DIRZ_g = [0;0;1];             %大地座標系中Z軸方向的單位向量

dis = Echo_time*c/2;         %得到各換能器到地面的距離

H1 = dis(1)*DIRO1_g'*DIRZ_g;  %由1號換能器估計的距離
H2 = dis(2)*DIRO2_g'*DIRZ_g;  %由2號換能器估計的距離
H3 = dis(3)*DIRO3_g'*DIRZ_g;  %由3號換能器估計的距離
H4 = dis(4)*DIRO4_g'*DIRZ_g;  %由4號換能器估計的距離

H = (H1+H2+H3+H4)/4;       %得到最終的高度

function [vg,vd] = Get_absolute_velocity(alpha,beta,vb,attitude_angle)
% ************************ ADCP流速座標系轉換 ******************************
% 依據Janus陣型結構四子陣測得的徑向多普勒速度,根據轉換矩陣B轉換得到基陣座標系下
% 的速度,然後再依據轉換矩陣R得到大地座標系下的速度.
% 參考文獻：[1] 劉德鑄,聲學多普勒流速測量關鍵技術研究,2.4節.
%                    ∧
%           子陣1    ||y    子陣2
%                    ||
% 行船方向 <-----x---||---------
%                    ||
%           子陣4    ||     子陣3
%                    ||
% **** 輸入參數
% **   alpha           換能器軸線方向與換能器陣軸線方向夾角，即安裝角，單位：°
% **   beta            X型Janus陣型結構逆時針與座標軸的夾角           單位：°
% **   vb              各換能器的徑向速度                            單位：m/s
% **   attitude_angle  姿態角                                        單位：°
% **** 輸出參數
% **   vg              輸出對地速度                                  單位：m/s


alpha = alpha*pi./180;% 與z軸的夾角
beta = beta*pi./180;% X型Janus陣型結構逆時針與座標軸的夾角
B = [sin(alpha)*cos(beta),sin(alpha)*sin(beta),cos(alpha);
    -sin(alpha)*sin(beta),sin(alpha)*cos(beta),cos(alpha);
    -sin(alpha)*cos(beta),-sin(alpha)*sin(beta),cos(alpha);
    sin(alpha)*sin(beta),-sin(alpha)*cos(beta),cos(alpha)];
% vb = [1 1 1 1]';% 換能器測出多普勒徑向速度 
vd = pinv(B)*vb;% 基陣座標系下的速度

fai =   attitude_angle(1)*pi./180;% 橫搖 roll 沿x軸順時針旋轉
theta = attitude_angle(2)*pi./180;% 縱搖 pitch 沿y軸順時針旋轉
pfai =  attitude_angle(3)*pi./180;% 艏搖 yaw 偏航 沿z軸順時針旋轉

Fai = [1,0,0;
       0,cos(fai),sin(fai);
       0,-sin(fai),cos(fai)];
Theta = [cos(theta),0,-sin(theta);
        0,1,0;
        sin(theta),0,cos(theta)];
Pfai = [cos(pfai),sin(pfai),0;
        -sin(pfai),cos(pfai),0;
        0,0,1];
R = Pfai*Theta*Fai;% 座標轉換矩陣

vg = R*vd;% 大地座標系下的速度

運行得到結果如下：