多傳感器分布式融合算法——多傳感器網(wǎng)絡(luò)協(xié)同目標(biāo)跟蹤和定位

這篇具有很好參考價值的文章主要介紹了多傳感器分布式融合算法——多傳感器網(wǎng)絡(luò)協(xié)同目標(biāo)跟蹤和定位。希望對大家有所幫助。如果存在錯誤或未考慮完全的地方，請大家不吝賜教，您也可以點擊"舉報違法"按鈕提交疑問。

多傳感器分布式融合算法

應(yīng)用：

多傳感器網(wǎng)絡(luò)協(xié)同目標(biāo)跟蹤及定位

原創(chuàng)不易，路過的各位大佬請點個贊

主要講解算法：
? ? ? ? ?多傳感器集中式融合算法/分布式融合算法/序貫融合算法
? ? ? ? ?多速率多傳感器異步融合算法
? ? ? ? ?多傳感器網(wǎng)絡(luò)分布式一致濾波

應(yīng)用于: 多傳感器網(wǎng)絡(luò)協(xié)同跟蹤/定位/導(dǎo)航

聯(lián)系WX: ZB823618313

1. 多傳感器數(shù)據(jù)融合系統(tǒng)結(jié)構(gòu)

? ? ? ? ?根據(jù)系統(tǒng)需求(成本、安全性、可維護性等)以及外界環(huán)境(自然環(huán)境、人為對抗環(huán)境),信息融合系統(tǒng)的結(jié)構(gòu)一般可劃分為:集中式結(jié)構(gòu)、分布式結(jié)構(gòu)以及混合式結(jié)構(gòu),下面以目標(biāo)跟蹤為背景,分別介紹這三種結(jié)構(gòu)。

1.1 集中式融合

? ? ? ? ?集中式結(jié)構(gòu)的特點是將各個傳感器的量測傳給融合中心,由融合中心統(tǒng)一進行目標(biāo)跟蹤處理。所以集中式融合也稱之為中心式融合或量測融合。該結(jié)構(gòu)充分利用了信源的信息,系統(tǒng)信息損失小,性能比較好,但系統(tǒng)對通信帶寬要求較高,系統(tǒng)的可靠性較差。

? ? ? ? ?集中式融合可以得到最優(yōu)的融合結(jié)果。但這種結(jié)構(gòu)需要頻帶很寬的數(shù)據(jù)傳輸鏈路來傳輸原始數(shù)據(jù),并且需要有較強處理能力的中心處理器,所以工程上實現(xiàn)起來較為困難。

多傳感器綜合定位,多傳感器融合/多傳感器協(xié)同跟蹤/定位,分布式融合,多傳感器融合,目標(biāo)跟蹤,matlab,協(xié)同跟蹤

圖1：集中式融合

集中式融合一般包含：
1）量測堆疊融合（批處理方式）
2）序貫處理融合
3）數(shù)據(jù)壓縮

1.2 分布式融合

? ? ? ? ?分布式融合(distributed fusion)也稱為傳感器級融合或自主式融合。在這種結(jié)構(gòu)中,每個傳感器都有自己的處理器,進行一些預(yù)處理,然后把中間結(jié)果送到中心結(jié)點,進行融合處理。由于各傳感器都具有自己的局部處理器,能夠形成局部航跡，所以在融合中心也主要是對各局部航跡進行融合，因此這種融合方法通常也稱為航跡融合( track fusion)。這種結(jié)構(gòu)因?qū)π诺廊萘恳蟮?系統(tǒng)生命力強,工程上易于實現(xiàn)而受到很大重視v并成為信息融合研究的重點。分布式航跡融合系統(tǒng)根據(jù)其通信方式的不同又可分為:

? ? ? ? ?1）無反饋分層融合結(jié)構(gòu)：各傳感器結(jié)點把各自的局部估計全部傳送到中心結(jié)點以形成全局估計,這是最常見的分布式融合結(jié)構(gòu)。

多傳感器綜合定位,多傳感器融合/多傳感器協(xié)同跟蹤/定位,分布式融合,多傳感器融合,目標(biāo)跟蹤,matlab,協(xié)同跟蹤

圖2：無反饋分層融合結(jié)構(gòu) \\

? ? ? ? ?2）有反饋分層融合結(jié)構(gòu)：在這種結(jié)構(gòu)中,中心結(jié)點的全局估計可以反饋到各局部結(jié)點,它具有容錯的優(yōu)點。當(dāng)檢測出某個局部結(jié)點的估計結(jié)果很差時,不必把它排斥于系統(tǒng)之外,而是可以利用全局結(jié)果來修改局部結(jié)點的狀態(tài)。這樣既改善了局部結(jié)點的信息,又可繼續(xù)利用該結(jié)點的信息。文獻5證明了此種結(jié)構(gòu)并不能改善融合系統(tǒng)的性能,但可以提高局部估計的精度。
多傳感器綜合定位,多傳感器融合/多傳感器協(xié)同跟蹤/定位,分布式融合,多傳感器融合,目標(biāo)跟蹤,matlab,協(xié)同跟蹤

圖3：有反饋分層融合結(jié)構(gòu) \\

? ? ? ? ?分布式結(jié)構(gòu)的特點是先由各個信源模塊對所獲取的量測進行跟蹤處理,然后再對各個傳感器形成的目標(biāo)航跡進行融合。該結(jié)構(gòu)的信息損失大于集中式結(jié)構(gòu),性能較集中式略差,但可靠性高,并且對系統(tǒng)通信帶寬要求不高。

從不同的角度，分布式融合也可以分為不同類型。分布式結(jié)構(gòu)也可以分為三種形式:
1）有融合中心的分布式結(jié)構(gòu)
2）無融合中心,共享航跡的分布式結(jié)構(gòu)
3）無融合中心,共享關(guān)聯(lián)量測的分布式結(jié)構(gòu)。`

筆記：

多傳感器分布式融合也是估計融合領(lǐng)域最熱門的鄰域之一，也是目前兩大學(xué)者在融合鄰域關(guān)注的焦點

此外，分布式融合算法種類非常多，研究角度也各不相同。換句話說，分布式融合本身就是一個非常大的方向

分布式融合也是本博客主要介紹的融合方式之一。本博客主要介紹分布式融合中最常用、最普遍的一種算法。

1.3 混合式融合

? ? ? ? ?混合式結(jié)構(gòu)是集中式和分布式兩種結(jié)構(gòu)的組合,同時傳送各個信源的量測以及各個信源經(jīng)過跟蹤處理的航跡,綜合融合量測以及目標(biāo)航跡。該結(jié)構(gòu)保留了集中式和分布式兩種結(jié)構(gòu)的優(yōu)點,但在通信帶寬、計算量、存儲量上一般要付出更大的代價。

2. 分布式融合結(jié)構(gòu)及對分布式融合理解

·按理說，應(yīng)該先寫集中式融合的博客。但是，不重要。哈哈哈哈哈哈哈

先上分布式融合兩個定義。當(dāng)然，我也不知道這兩個定義對大家有沒有用。反正定義是抄的。

? ? ? ? ?定義1 在多傳感器融合系統(tǒng)中,每個傳感器的跟蹤器所給出的航跡稱作局部航跡(local track)或傳感器航跡(sensor track)。

? ? ? ? ?定義2 航跡融合系統(tǒng)將各個局部航跡或傳感器航跡融合后形成的航跡稱作系統(tǒng)航跡(system track)或全局航跡(global track)。當(dāng)然,將局部航跡或傳感器航跡與系統(tǒng)航跡融合后形成的航跡仍然稱為系統(tǒng)航跡。

2.1 分布式融合結(jié)構(gòu)及深入理解

? ? ? ? ?分布式融合結(jié)構(gòu)中研究最廣泛，關(guān)注度最高、挑戰(zhàn)最大的就是航跡融合。所謂航跡融合，就是各個傳感器向融合中心或者別的傳感器傳送目標(biāo)的“航跡”，而不是傳感器本身直接觀測得到的數(shù)據(jù)。

? ? ? ? ?這里有個提醒的是，很多人把分布式融合與航跡融合等價。實際上，分布式融合包含了航跡融合。航跡融合只是分布式融合的一個子集。只是航跡融合應(yīng)用最廣泛而已

? ? ? ? ?還有一種分布式融合，各個局部傳感器向融合中心或者其它傳感器傳輸?shù)牟皇恰昂桔E”，也不是“原始量測”，而是一種處理過的量測。多為處理過的量測，表現(xiàn)形式有很多種型式，一般可以稱它們?yōu)閭瘟繙y。這種分布式融合的論文其實也很多，研究前景也很大。

? ? ? ? ?還還還有一種分布式融合，哈哈哈哈哈，其實。該種分布式融合是那種呢？它們是集中式融合的變體。因此有著一般有著最優(yōu)的融合性能。
比較典型的有：

有反饋分布式融合；

無反饋分布式融合；

聯(lián)邦卡爾曼濾波

信息濾波器。
不過這種分布式融合，有學(xué)者認為，它們是集中式融合往航跡融合的變體，目的地還是航跡融合

? ? ? ? ?本博客還是主要介紹分布式融合中的航跡融合。

? ? ? ? ? 航跡融合包含兩步:航跡關(guān)聯(lián)和航跡狀態(tài)估計融合。在航跡關(guān)聯(lián)過程中,來自不同傳感器的航跡進行關(guān)聯(lián)以形成系統(tǒng)航跡,每個傳感器的航跡相應(yīng)于一個單獨的假定的目標(biāo)。給定了一個關(guān)聯(lián)過程之后,系統(tǒng)航跡的狀態(tài)估計就可以通過融合關(guān)聯(lián)上的傳感器航跡的狀態(tài)估計來得到。

? ? ? ? ?根據(jù)是否利用系統(tǒng)航跡的狀態(tài)估計,航跡融合通常有兩種可能的處理結(jié)構(gòu)。

多傳感器綜合定位,多傳感器融合/多傳感器協(xié)同跟蹤/定位,分布式融合,多傳感器融合,目標(biāo)跟蹤,matlab,協(xié)同跟蹤

圖4：航跡融合系統(tǒng)結(jié)構(gòu) \\

2.2 航跡融合結(jié)構(gòu)分類

2.2.1 傳感器到傳感器的航跡融合

? ? ? ? ?傳感器到傳感器的航跡融合結(jié)構(gòu)如圖6-3-2所示,可以看出,來自不同傳感器航跡(傳播到同一時刻)的狀態(tài)估計互相之間進行關(guān)聯(lián)和融合以得到系統(tǒng)航跡的狀態(tài)估計。在這一過程中不利用系統(tǒng)航跡以前的狀態(tài)估計。要注意的是,對于這種結(jié)構(gòu),融合一般來說包含來自多于兩個傳感器的航跡集合。

多傳感器綜合定位,多傳感器融合/多傳感器協(xié)同跟蹤/定位,分布式融合,多傳感器融合,目標(biāo)跟蹤,matlab,協(xié)同跟蹤

圖5：傳感器到傳感器的航跡融合 \\

? ? ? ? ?這種結(jié)構(gòu)不必處理相關(guān)估計誤差的問題(如果忽略掉共同的先驗信息)。由于它基本上是一個無記憶的操作,所以關(guān)聯(lián)和航跡估計融合中的誤差不會從一個時刻傳播到下一時刻。然而,由于過去的處理結(jié)果被丟棄掉了,這一方法可能沒有傳感器到系統(tǒng)的融合結(jié)構(gòu)有效。

2.2.2 傳感器到系統(tǒng)的航跡融合

? ? ? ? ?無論什么時候接收到一組傳感器航跡﹐系統(tǒng)航跡的狀態(tài)估計被外推到傳感器航跡的時刻,并且與最新接收到的傳感器航跡進行融合。當(dāng)接收到另外一組傳感器航跡時,重復(fù)這一過程。

? ? ? ? ?傳感器到系統(tǒng)的航跡融合把關(guān)聯(lián)問題簡化成-一個bi-partite分配問題,所以可以利用常見的分配算法。然而,它必須處理相關(guān)估計誤差的問題。在圖6-3-3中,A點的傳感器航跡和B點的系統(tǒng)航跡具有相關(guān)的誤差,原因是它們都依賴于C點。進一步說,由于關(guān)聯(lián)或融合中過去處理誤差導(dǎo)致的系統(tǒng)航跡中的任何誤差會影響將來的融合性能。

多傳感器綜合定位,多傳感器融合/多傳感器協(xié)同跟蹤/定位,分布式融合,多傳感器融合,目標(biāo)跟蹤,matlab,協(xié)同跟蹤
center>圖6：傳感器到系統(tǒng)的航跡融合 \

3. 分布式融合困難的原因分析

3.1 航跡融合中各傳感器局部估計誤差相關(guān)

如果各傳感器局部估計誤差之間互不相關(guān),相應(yīng)的融合算法非常簡單。

但在一般情況下,這一假設(shè)是不滿足的,即各傳感器的局部估計誤差之間是相關(guān)的,具體原因如下所述。

3.1.1 共同的先驗估計

? ? ? ? ?這種情況發(fā)生在傳感器到系統(tǒng)的航跡融合結(jié)構(gòu)中

? ? ? ? ?要么是一個量測和一條航跡的融合,要么是一條航跡和另外一條航跡的融合。假定航跡已經(jīng)被傳播到一個共同的時刻?；旧蟻碚f,在一個結(jié)點的一條航跡會包含祖先結(jié)點（航跡和量測）中的所有信息。傳感器航跡估計和系統(tǒng)航跡估計，都包含從較早時刻傳播來的傳感器航跡估計。因此，即使兩條傳感器航跡不共享共同的先驗估計(除共同的先驗)，但是隨著遞推傳播，傳感器的航跡和系統(tǒng)航跡由于共同的先驗信息，必然存在相關(guān)。一般來說,如果在信息圖中從量測到融合結(jié)點存在多徑，也存在這一信息源引起的相關(guān)。

3.1.2 共同的過程噪聲

? ? ? ? ?這種情況甚至發(fā)生在傳感器航跡不共享共同的量測而進行融合時

? ? ? ? ?當(dāng)目標(biāo)動態(tài)方程不確定時,給定某一時刻的目標(biāo)狀態(tài),來自兩條傳感器航跡的量測不一定條件獨立。這樣一來,來自兩條傳感器航跡的估計誤差就可能不獨立。

? ? ? ? ?假定有兩條航跡 $i$ 和 $j$ 狀態(tài)估計(估計或預(yù)測)和相應(yīng)的誤差協(xié)方差矩陣(都傳播到同一時刻)分別為： $\hat{x}_i$ 和 $\hat{x}_j$ ， ${P}_i$ 和 ${P}_j$ 。估計融合問題就是找到最好的融合估計i和誤差協(xié)方差矩陣 ${P}$ 。這兩條航跡可能是一個傳感器到傳感器航跡融合結(jié)構(gòu)中的兩條航跡,或者是一個傳感器到系統(tǒng)航跡融合結(jié)構(gòu)中的一條系統(tǒng)航跡和一條傳感器航跡。

還有兩個導(dǎo)致航跡融合中各傳感器局部估計誤差相關(guān)的原因分別為：
各局部傳感器觀測誤差相關(guān)
各局部航。跡采用的目標(biāo)動力學(xué)建模是一樣的
這兩個原因在下面單獨分析，因為它們還會導(dǎo)致的困難不僅僅是使局部估計誤差相關(guān)

3.2 各傳感器量測噪聲相關(guān)

? ? ? ? ?基于卡爾曼濾波的集中式/分布式融合框架要求，不同來源的觀測誤差必須是不相關(guān)的。在許多實際情況下，這是一個真正的限制。例如，許多傳感器誤差依賴于常見的隨機估計對象(即待估計的數(shù)量)，因此是相關(guān)的。另一個例子是，在一個共同的噪聲環(huán)境中，被估計對象被多個傳感器觀測到，例如在由目標(biāo)產(chǎn)生的噪聲干擾期間。

傳感器噪聲相關(guān)，直接導(dǎo)致局部航跡相關(guān)。

3.3 局部動態(tài)模型不同

? ? ? ? ?實際上，現(xiàn)在基本上所有的跟蹤融合論文，都默認局部估計的動力學(xué)模型和融合中心的動力模型一致。也就是說，融合中，過程煙花模型只是一個。這顯然不能符合分布式的思想。分布式各局部估計中心，一般應(yīng)該擁有自己的建模。

? ? ? ? ? 基于卡爾曼濾波的集中式融合框架要求，局部動態(tài)模型必須相同。在現(xiàn)實中，局部估計可能是基于被評估者的不同動態(tài)模型來獲得的，以考慮來源特定的情況或需求。對局部估計器使用不同的動態(tài)模型可能是必要的或更有效的。這是必要的，例如，當(dāng)狀態(tài)增強用于一些具有自相關(guān)傳感器觀測噪聲的局部估計。例如，當(dāng)一個局部估計器中使用多個模型時，它可能更有效。為了獲得最好的融合性能，局部模型的選擇當(dāng)然不是隨意的。

3.4 分布式系統(tǒng)的網(wǎng)絡(luò)結(jié)構(gòu)和信息模式復(fù)雜

? ? ? ? ?基于卡爾曼濾波的集中式/分布式融合框架要求，分布式系統(tǒng)的網(wǎng)絡(luò)結(jié)構(gòu)和信息模式必須簡單。這種局限性顯然源于這樣一個事實:當(dāng)分布式系統(tǒng)過于復(fù)雜時，實際上不可能通過使用局部估計重構(gòu)集中式融合規(guī)則來設(shè)計融合規(guī)則。

4. 分布式融合算法總結(jié)

? ? ? ? ? 下面只是簡單羅列比較經(jīng)典且常用的分布式融合算法。針對這些算法的細節(jié)及實驗，在后續(xù)的博客中具體講解，

? ? ? ? ? 如果等不急，可以直接聯(lián)系博主。

4.1 簡單凸組合SCC融合算法

4.2 Bar-Shalom-Campo融合算法

4.3 協(xié)方差交叉CI融合算法

4.4 分布式信息濾波器算法

4.5 帶反饋的最優(yōu)分布式融合算法

4.6 不帶反饋的最優(yōu)分布式融合算法

4.7 最大后驗概率MAP融合算法

4.8 最大似然概率ML融合算法

4.9 最大信息熵KL融合算法

4.10 聯(lián)邦卡爾曼濾波器算法

4.11 基于最優(yōu)統(tǒng)一融合準(zhǔn)則的融合算法

4.11.1 最優(yōu)線性無偏估計融合BLUE

4.11.2 加權(quán)最小二乘融合WLS

備注：如果大家還了解其它的比較常用的分布式融合算法，可以在評論區(qū)留言。
**? ? ? ? ?大家如果還有其它想了解的融合算法，可以在評論區(qū)說出來。 **

5. 基于分布式融合的交互式多模型IMM機動目標(biāo)跟蹤

講了這么多，還不如直接弄一個實例。

既然是第一篇關(guān)于分布式融合的博客，實例直接上比較復(fù)雜的。哈哈哈哈哈哈

5.1 算法種類

算法：基于簡單凸組合SCC的交互式多模型無跡卡爾曼濾波
? ? ? ? ??基于協(xié)方差交叉CI的交互式多模型無跡卡爾曼濾波
? ? ? ? ??交互式多模型分布式無跡信息濾波融合
? ? ? ? ??基于協(xié)方差交叉CI的交互式多模型粒子濾波

5.2 參數(shù)設(shè)置

運動模型： 近勻速CV模型 $\dot{x}, \dot{y}]^T$
$X_{k+1}=\begin{bmatrix}1&0&T&0\\0&1&0&T\\0&0&1&0\\0&0&0&1 \end{bmatrix}X_{k} + W_k$
其中 $W_k$ 為零均值白噪聲，其方差為：
$Q_k=q_k^2\begin{bmatrix}T^3/3&T^2/2&0&0 \\T^2/2&T&0&0 \\0&0&T^3/3&T^2/2 \\0&0& T^2/2&T\end{bmatrix}$
? ? ? ? ??
僅勻轉(zhuǎn)彎CT模型 $\dot{x}, \dot{y}, w]^T$
$X_{k+1}=\begin{bmatrix}1&\frac{\sin(\omega T)}{\omega}&0&-\frac{1-\cos(\omega T)}{\omega}&0\\0&\cos(\omega T)&0&-\sin(\omega T)&0\\0&\frac{1-\cos(\omega T)}{\omega}&1&\frac{\sin(\omega T)}{\omega}&0\\0&\sin(\omega T)&0&\cos(\omega T)&0\\0&0&0&0&1\end{bmatrix}X_{k} + W_k$
其中 $W_k$ 為零均值白噪聲，其方差為：
$Q_k=q_k^2\begin{bmatrix}T^3/3&T^2/2&0&0& 0 \\T^2/2&T&0&0 &0\\0&0&T^3/3&T^2/2 &0 \\0&0& T^2/2&T&0\\0&0&0&0&q_w\end{bmatrix}$
或者為(兩種形式都可以用，下面一代碼形式給出)
? ? ? ? ??近勻加速CA模型 $\dot{x},\dot{y},\ddot{x}, \ddot{y}]^T$
$X_{k+1}=\begin{bmatrix}1&0&T&0&T^2/2&0\\0&1&0&T&0&T^2/2\\0&0&1&0&T&0 \\0&0&0&1&0&T \\0&0&0&0&1&0 \\0&0&0&0&0&1 \end{bmatrix}X_{k} + W_k$
其中 $W_k$ 為零均值白噪聲，其方差為：

Qk3=q3^2*[T^5/20   0      T^4/8  0       T^3/6   0;
          0       T^5/20  0      T^4/8   0       T^3/6;
          T^4/8   0       T^3/3  0       T^2/2   0;
          0       T^4/8   0      T^3/3   0       T^2/2;
          T^3/6   0       T^2/2  0       T       0
          0       T^3/6   0      T^2/2   0       T];

傳感器： 多個主動雷達
在二維情況下，雷達量測為距離和角度
${r}_k^m=r_k+\tilde{r}_k\\ b^m_k=b_k+\tilde_k$
其中
$r_k=\sqrt{(x_k-x_0)^+(y_k-y_0)^2)}\\ b_k=\tan^{-1}{\frac{y_k-y_0}{x_k-x_0}}\\$
$x_0,y_0]$ 為雷達坐標(biāo)，一般情況為0。雷達量測為 $z_k=[r_k,b_k]'$ 。雷達量測方差為
$R_k=\text{cov}(v_k)=\begin{bmatrix}\sigma_r^2 & 0 \\0 & \sigma_b^2 \end{bmatrix}$

5.3 實驗結(jié)果

5.3.1 跟蹤軌跡

多傳感器綜合定位,多傳感器融合/多傳感器協(xié)同跟蹤/定位,分布式融合,多傳感器融合,目標(biāo)跟蹤,matlab,協(xié)同跟蹤

5.3.2 位置均方誤差RMSE

多傳感器綜合定位,多傳感器融合/多傳感器協(xié)同跟蹤/定位,分布式融合,多傳感器融合,目標(biāo)跟蹤,matlab,協(xié)同跟蹤

5.3.3 速度均方誤差RMSE

多傳感器綜合定位,多傳感器融合/多傳感器協(xié)同跟蹤/定位,分布式融合,多傳感器融合,目標(biāo)跟蹤,matlab,協(xié)同跟蹤

5.3.4 加速度均方誤差RMSE

多傳感器綜合定位,多傳感器融合/多傳感器協(xié)同跟蹤/定位,分布式融合,多傳感器融合,目標(biāo)跟蹤,matlab,協(xié)同跟蹤

5.3.5 角速度均方誤差RMSE

多傳感器綜合定位,多傳感器融合/多傳感器協(xié)同跟蹤/定位,分布式融合,多傳感器融合,目標(biāo)跟蹤,matlab,協(xié)同跟蹤

6. 代碼

代碼僅為部分代碼，也是一個案例。
如果需要分布式融合算法代碼，和論文復(fù)現(xiàn)，WX：ZB823618313


close all;
clear all;
clc;
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%系統(tǒng)參數(shù)設(shè)置
runs=5; %蒙特卡洛實驗次數(shù)，濾波將進行50次
steps=140; %跟蹤總時長
M=5;% 雷達數(shù)量
nz=2;% 測量維數(shù)
T=1;% 采樣時間
%模型1：CV模型x=[x位置， y位置， x速度， y速度]'：的動態(tài)方程參數(shù)設(shè)置,Xk+1=fk(Xk)+G*uk+Gk*Wk,CV模型
Fk_cv=[1 0 T 0;
       0 1 0 T;
       0 0 1 0;
       0 0 0 1];% 
q1=0.01; % 目標(biāo)運動學(xué)標(biāo)準(zhǔn)差，過程噪聲
% % Qk1=q1*[T^3/3, 0,     T^2/2,   0;
% %           0      T^3/3, 0        T^2/2;
% %           T^2/2, 0      T^2        0
% %           0      T^2/2  0        T^2];% 過程噪聲協(xié)方差
% % Gk_cv= eye(4); %過程噪聲增益矩陣
Gk_cv= [ T^2/2  0
       0     T^2/2      
       T       0
       0      T]; %過程噪聲增益矩陣
Qk1=q1^2*eye(2);
%模型2：CT模型x=[x位置， y位置， x速度， y速度， 角速度]' 的動態(tài)方程參數(shù)設(shè)置,Xk+1=fk(Xk)+G*uk+Gk*Wk,CT模型
Fk_ct=@(w1)[1           0      sin(w1*T)/w1      -(1-cos(w1*T))/w1   0;
       0         1      (1-cos(w1*T))/w1   sin(w1*T)/w1       0;
       0         0      cos(w1*T)         -sin(w1*T)          0;
       0         0      sin(w1*T)          cos(w1*T)          0
       0         0      0                  0                  1;];% CT模型的系統(tǒng)矩陣是關(guān)于狀態(tài)x(5)的函數(shù)，即轉(zhuǎn)彎角速度w的函數(shù)
q2 = 0.01;
q_ctw=1.75e-8;% CT模型角速度過程噪聲方差
% 角速度定常數(shù)


randn('state',sum(100*clock));  %每次給不同的狀態(tài)重置隨機數(shù)產(chǎn)生器（因為clock每次都不同）

%%

for index=1:runs %蒙特卡洛次數(shù)
    index         %顯示運行次數(shù)
    %濾波初始化設(shè)置
    % CV
    X_cv=[30000,20000,80,50]';
    P_cv=diag([1e5,1e5,1e3,1e3]);
    % CT
    w1 = 4.5*pi/180;% 初始角速度
    w2 = -6*pi/180;% 初始角速度
    X_ct=[30000,20000,80,50 w1]';
    P_ct=diag([1e5,1e5,1e3,1e3,1e-6]);
    % CA
    acc=[6;5];
    X_ca=[30000,20000,80,50, acc']';
    P_ca=diag([1e5,1e5,1e3,1e3,1e-2,1e-2]);
    %濾波初始化
    X_cv_zero=X_cv+sqrtm(P_cv)*randn(4,1);%產(chǎn)生真實X0
    X_ct_zero=X_ct+sqrtm(P_ct)*randn(5,1);%產(chǎn)生真實X0
    X_ca_zero=X_ca+sqrtm(P_ca)*randn(6,1);%產(chǎn)生真實X0
%IMMUKF三個濾波器的初始化   
    xk_UKFcv=X_cv_zero;             %X(0|0)= X_aver_zero
    Pk_UKFcv=P_cv;                  %P(0|0)= P_zero
    xk_UKFct=X_ct_zero;
    Pk_UKFct=P_ct;
    xk_UKFca=X_ca_zero;
    Pk_UKFca=P_ca;
    for m=1:M
        xk_UKF{m}={xk_UKFcv, xk_UKFct, xk_UKFca};
        Pk_UKF{m}={Pk_UKFcv, Pk_UKFct, Pk_UKFca};

原創(chuàng)不易，路過的各位大佬請點個贊文章來源地址http://www.zghlxwxcb.cn/news/detail-808232.html

到了這里，關(guān)于多傳感器分布式融合算法——多傳感器網(wǎng)絡(luò)協(xié)同目標(biāo)跟蹤和定位的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！