考慮噪聲特性未知的多傳感器姿態(tài)融合方法

王 碩，宋申民，于 浛

（哈爾濱工業(yè)大學(xué) 控制理論與制導(dǎo)技術(shù)研究中心，哈爾濱 150001）

考慮噪聲特性未知的多傳感器姿態(tài)融合方法

王 碩，宋申民，于 浛

（哈爾濱工業(yè)大學(xué) 控制理論與制導(dǎo)技術(shù)研究中心，哈爾濱 150001）

研究了噪聲特性和互協(xié)方差未知情況下多傳感器系統(tǒng)的協(xié)方差交叉融合估計(jì)問題，其中噪聲特性未知是指系統(tǒng)的狀態(tài)噪聲和量測噪聲的相關(guān)性和真實(shí)方差均是未知的。首先，每個(gè)子系統(tǒng)根據(jù)最新更新時(shí)刻的測量信息，選擇相應(yīng)的CKF估計(jì)器，從而得到各子系統(tǒng)在每一時(shí)刻的一個(gè)局部估計(jì)。其次，以矩陣加權(quán)線性最小方差為最優(yōu)融合準(zhǔn)則，提出了一種快速連續(xù)的協(xié)方差交叉(SCI)融合策略，將多維優(yōu)化問題簡化到對幾個(gè)一維非線性代價(jià)函數(shù)的優(yōu)化，得到最優(yōu)融合估計(jì)。再次，在子系統(tǒng)的估計(jì)中采用誤差四元數(shù)和誤差修正羅德里格斯參數(shù)相互切換的方法來避免四元數(shù)的規(guī)范約束和誤差修正羅德里格斯參數(shù)奇異的發(fā)生。最后，通過一個(gè)衛(wèi)星姿態(tài)估計(jì)的例子驗(yàn)證了所提方法的有效性。

CKF估計(jì)；信息融合；協(xié)方差交叉；四元數(shù)；羅德里格斯參數(shù)

隨著航天器和空間系統(tǒng)復(fù)雜性的不斷加大，需要對航天器的姿態(tài)進(jìn)行更加準(zhǔn)確和魯棒的估計(jì)。對每個(gè)航天器的功能需求正在增加，而航天器本身的尺寸卻越來越小，且硬件之間是相互獨(dú)立的，這就導(dǎo)致系統(tǒng)的計(jì)算能力受到很大限制。這樣的系統(tǒng)常常被稱是操作響應(yīng)空間(ORS)。ORS系統(tǒng)的主要目標(biāo)是為了航天器使用模塊化的類型來建設(shè)加速任務(wù)概念推出的時(shí)間。這種模塊化的系統(tǒng)，定制的接口和軟件必須管理傳送到中央處理器的所有數(shù)據(jù)。然而，潛在的危險(xiǎn)情況是數(shù)據(jù)只傳播一部分，并不是全部。處理器是一個(gè)大型的分布式系統(tǒng)。由于數(shù)據(jù)不能遍布每個(gè)處理器進(jìn)行運(yùn)算，從這個(gè)角度上說估計(jì)結(jié)果是次優(yōu)的。在理想的情況下，有可能把所有的次優(yōu)估計(jì)通過某種方式結(jié)合起來而處理所有數(shù)據(jù)，從而得到最優(yōu)解。這樣做的一個(gè)主要缺陷是有多個(gè)估計(jì)，而每個(gè)估計(jì)之間的相關(guān)性是無法確定的。

為了克服此數(shù)據(jù)融合問題，一個(gè)簡單而有效的方法是采用協(xié)方差交叉(CI)算法[1-2]。CI算法通過考慮誤差空間的交叉來克服未知的相關(guān)性問題，而獲得一個(gè)保守但卻一致的融合結(jié)果。文獻(xiàn)[3]表明，CI算法可以被視為一種優(yōu)化問題的解決方案，使得單個(gè)估計(jì)和融合估計(jì)之間的權(quán)重誤差最小。而對多傳感器數(shù)據(jù)通過批處理獲得魯棒的CI融合估計(jì)，魯棒CI融合的獲得是基于多維非線性代價(jià)函數(shù)的優(yōu)化，這需要更大的計(jì)算負(fù)擔(dān)[1,4]。在特殊情況下，魯棒 CI融合的一個(gè)快速近似解析解在文獻(xiàn)[4]中獲得。為了減少復(fù)雜性和計(jì)算負(fù)擔(dān)，通過連續(xù)處理，基于連續(xù)的兩傳感器CI融合容積卡爾曼濾波器，提出了一個(gè)快速連續(xù)的協(xié)方差交叉(SCI)容積卡爾曼融合算法。這是一個(gè)快速遞歸的兩個(gè)傳感器CI融合，是把多維優(yōu)化問題減少到對幾個(gè)一維非線性代價(jià)函數(shù)的優(yōu)化。而一維優(yōu)化問題則可以通過黃金節(jié)分割點(diǎn)方法或斐波那契法簡單的解決。

在局部估計(jì)中采用四元數(shù)作為姿態(tài)的參數(shù)化描述，給出了一些解決辦法，解決四元數(shù)的CI方程，同時(shí)保持約束性。然而，這種情況下狀態(tài)向量融合包括多個(gè)四元數(shù)，文獻(xiàn)[3]的方法不能得到廣義的 CI解決方案，因?yàn)闋顟B(tài)必須服從多個(gè)二次等式約束。Nebelecky等人在文獻(xiàn)[5]中給出了一個(gè)解決辦法，使用迭代的Newton-Raphson方法來確定最優(yōu)融合估計(jì)。解決四元數(shù) CI融合問題的主要困難在于保持四元數(shù)的規(guī)范約束。避免這個(gè)問題最為直接的方法是使用三維的姿態(tài)參數(shù)描述，如羅德里格斯參數(shù)和修正羅德里格斯參數(shù)[6-8]。自1965年Wahba問題提出以來，四元數(shù)就一直是姿態(tài)參數(shù)化首選的描述方式[9]。然而，近些年修正羅德里格斯參數(shù)(MRPs)也已經(jīng)被廣泛應(yīng)用于航天器的姿態(tài)估計(jì)和控制當(dāng)中。Karlgaar等在文獻(xiàn)中給出了經(jīng)典的修正羅德里格斯參數(shù)[10]，這些參數(shù)是由四元數(shù)到三維超平面立體投影的結(jié)果。需要指出的是，羅德里格斯參數(shù)和MRPs參數(shù)是對稱的球面參數(shù)的一部分。而所有的三維參數(shù)描述姿態(tài)又存在特定的奇異現(xiàn)象，這使問題變得更復(fù)雜了。解決這個(gè)問題的簡單有效方法是采用四元數(shù)和三維參數(shù)化表示相互切換進(jìn)行姿態(tài)的估計(jì)。這種方法在非線性估計(jì)技術(shù)中普遍使用，如容積卡爾曼濾波(CKF)和粒子濾波(PF)[11-15]。其中，CKF在對非線性系統(tǒng)的估計(jì)中表現(xiàn)尤為突出，在數(shù)值穩(wěn)定性上明顯優(yōu)于UKF，且濾波精度接近PF。

本文研究了噪聲特性未知情況下多傳感器系統(tǒng)的CKF融合估計(jì)問題，提供了一種新的SCI融合方法。將每組傳感器所對應(yīng)的局部估計(jì)系統(tǒng)建模成一個(gè)子系統(tǒng)。進(jìn)而，每個(gè)子系統(tǒng)根據(jù)最新測量信息的更新時(shí)刻，選擇相應(yīng)的CKF估計(jì)器，從而得到各子系統(tǒng)在每一時(shí)刻的局部最優(yōu)估計(jì)。然后根據(jù)矩陣加權(quán)最小方差融合準(zhǔn)則得到最優(yōu)的SCI融合估計(jì)。SCI融合的姿態(tài)使用誤差四元數(shù)和誤差修正羅德里格斯參數(shù)相互切換的估計(jì)方法獲得，避免了四元數(shù)描述參數(shù)的規(guī)范約束，同時(shí)也避免了濾波過程中奇異的發(fā)生。最后，通過一個(gè)姿態(tài)估計(jì)的例子驗(yàn)證本文所提方法的有效性和優(yōu)越性。

1 協(xié)方差交叉融合

1.1 CI融合過程

協(xié)方差交叉融合的相關(guān)估計(jì)已經(jīng)取得了廣泛應(yīng)用，本文將其應(yīng)用到航天器的姿態(tài)確定。研究中假設(shè)航天器上裝有兩個(gè)星敏感器，每個(gè)敏感器各自敏感測量值。每個(gè)星敏感器又都有各自的容積卡爾曼濾波處理的星測量值，兩個(gè)濾波器的狀態(tài)量使用同一組陀螺儀的輸出值，并且呈現(xiàn)出了相關(guān)性，系統(tǒng)結(jié)構(gòu)如圖1所示。使用CI算法，可以更好地將兩個(gè)估計(jì)進(jìn)行融合，獲得新的狀態(tài)向量xCI和協(xié)方差PCI。

圖1 CI融合結(jié)構(gòu)圖Fig.1 CI fusion structure

圖2 CI融合的幾何意義Fig.2 Geometric meaning of CI integration

圖 2顯示了由兩個(gè)測量協(xié)方差橢圓(實(shí)心橢圓)結(jié)合成一個(gè)融合協(xié)方差橢圓(虛線橢圓)的示例。當(dāng)個(gè)體之間的相關(guān)性估計(jì)精確已知時(shí)，最優(yōu)協(xié)方差可以完全重建，結(jié)果得到的橢圓完全在兩單個(gè)估計(jì)協(xié)方差橢圓的交點(diǎn)上。當(dāng)相關(guān)性未知且使用CI算法進(jìn)行融合時(shí)，獲得的融合協(xié)方差橢圓將通過兩單個(gè)協(xié)方差橢圓交叉的4個(gè)點(diǎn)，如圖2中所示。

1.2 問題描述

考慮如下帶有加性噪聲的非線性連續(xù)系統(tǒng)：

1.3 魯棒協(xié)方差交叉融合

這里加權(quán)參數(shù) θi的優(yōu)化過程是一個(gè)一維的優(yōu)化問題，可以采用0.618法或斐波納契法，也可以使用Matlab自帶的工具箱進(jìn)行運(yùn)算。

由式(4)到(6)可以得到：

上述方法很好地解決了互協(xié)方差未知情況下的信息融合問題。但是在多于兩個(gè)傳感器時(shí)，該方法的計(jì)算負(fù)擔(dān)較大。下面基于兩傳感器CI融合容積卡爾曼濾波器給出了一個(gè)快速連續(xù)的協(xié)方差交叉(SCI)融合容積卡爾曼濾波，這是一個(gè)快速遞歸兩個(gè)傳感器CI融合過程。

1.4 連續(xù)協(xié)方差交叉融合

為了減少在多傳感器情況下的復(fù)雜性和計(jì)算負(fù)擔(dān)，通過連續(xù)處理，基于(N-1)步兩傳感器 CI容積卡爾曼融合，給出了一個(gè)遞歸的 CI容積卡爾曼融合算法。(N-1)步兩傳感器 CI容積卡爾曼融合過程如圖 3所示，其具體的(N-1)步實(shí)現(xiàn)如下：

該算法如圖3所示。

圖3 連續(xù)協(xié)方差交叉融合結(jié)構(gòu)圖Fig.3 Consecutive covariance intersection structure

反復(fù)使用兩傳感器CI融合算法(4)到(6)，SCI容積卡爾曼融合可以通過兩傳感器CI融合遞歸實(shí)現(xiàn)。它包括N-1次兩傳感器CI容積卡爾曼融合，具體數(shù)學(xué)表示為

從式(12)可以看出，這是一個(gè)一維的優(yōu)化問題，可以通過黃金分割法或斐波那契法解決。因此，SCI融合算法的優(yōu)化過程就轉(zhuǎn)化為 N-1步的一維優(yōu)化問題，這樣易于實(shí)現(xiàn)，且增加了運(yùn)算的快速性。

1.5 連續(xù)協(xié)方差交叉融合算法的精度分析

定理 1 對于互協(xié)方差Pij未知的多傳感器系統(tǒng)(1)和(2)，SCI容積卡爾曼融合是一致的。即，對任意未知Pij有

式中，θ1∈[0,1]，其解的獲得是將優(yōu)化目標(biāo)函數(shù)最小。

引理 1 對于互協(xié)方差Pij未知的多傳感器系統(tǒng)(1)和(2)，SCI融合的整體處理表示為

定理 2 對于互協(xié)方差Pij未知的多傳感器系統(tǒng)(1)和(2)，的CI融合魯棒精度高于的SCI融合精度，

證明 由式(6)可以得到：

再由式(24)有：

2 航天器姿態(tài)運(yùn)動(dòng)模型和矢量觀測模型

2.1 姿態(tài)運(yùn)動(dòng)學(xué)模型

姿態(tài)運(yùn)動(dòng)學(xué)模型采用四元數(shù)描述可表示為

式中，ω表示角速度，矩陣Ω( ω)可用如下表示：

式中，ω=[ω1ω2ω3]T，[ω×]是ω的反對稱矩陣，表示為

2.2 陀螺輸出模型

假設(shè)陀螺固連在航天器上，并且陀螺的安裝方向與航天器本體坐標(biāo)系重合，可直接敏感航天器的角速度，則陀螺輸出模型可以表示為

式中，Δt代表步長，Nv與Nu是不相關(guān)的零均值高斯白噪聲。

2.3 矢量觀測模型

星敏感器通過觀測天體方向?qū)φ招菤v表來測定航天器的姿態(tài)。假設(shè)星敏感器的安裝方向與航天器本體坐標(biāo)系重合，則星光矢量在航天器本。體坐標(biāo)系下的觀測方程為

式中：r是星光矢量在慣性系下的單位矢量方向，可查詢星歷表獲得；是慣性系到星本體坐標(biāo)系的轉(zhuǎn)換矩陣；v是敏感器的觀測誤差，這里認(rèn)為是高斯白噪聲。假設(shè)有m個(gè)敏感器進(jìn)行觀測，則在第k時(shí)刻，用四元數(shù)描述的矢量觀測模型為

式中：bm和rm是第m個(gè)參考矢量分別在體系和慣性系下的分量；A(q)是姿態(tài)轉(zhuǎn)移矩陣，其四元數(shù)形式的描述為[18]

展開形式是

3 CKF姿態(tài)估計(jì)

在第二節(jié)中提出的SCI解決方案是代表一個(gè)全局性的方法，唯一的假設(shè)是修正羅德里格斯參數(shù)的存在。這種方法非常適合應(yīng)用在姿態(tài)是由修正羅德里格。斯參數(shù)直接估計(jì)的情況。然而，大多數(shù)的航天器應(yīng)用。中姿態(tài)是采用四元數(shù)表示的。在這種情況下修正羅德。里格斯參數(shù)只是用來完成航天器姿態(tài)的數(shù)據(jù)融合。

航天器采用式(33)給出的運(yùn)動(dòng)學(xué)方程描述，角速度來自陀螺的輸出值，系統(tǒng)的測量方程由式(38)表示，系統(tǒng)的狀態(tài)向量為。本文中對單個(gè)傳感器的估計(jì)過程以CKF為濾波框架，采用誤差四元數(shù)和誤差修正羅德里格斯參數(shù)相互切換的方法來對航天器姿態(tài)進(jìn)行估計(jì)。

3.1 時(shí)間更新

為了避免采用四元數(shù)描述姿態(tài)時(shí)存在的冗余導(dǎo)致濾波過程中協(xié)方差陣出現(xiàn)奇異的狀況，在此將狀態(tài)向量選為為與誤差四元數(shù)對應(yīng)的修正羅德里格斯參數(shù)：

由容積誤差四元數(shù)可以獲得一步預(yù)測估計(jì)的容。積四元數(shù)點(diǎn)集為

陀螺的角速度估計(jì)為

獲得下一步的容積四元數(shù)后，再使其轉(zhuǎn)換為容積誤差修正羅德里格斯參數(shù)。計(jì)算一步預(yù)測容積誤差四。元數(shù)得：

將容積點(diǎn)誤差四元數(shù)轉(zhuǎn)換為容積誤差修正羅德里格斯參數(shù)：

容積點(diǎn)誤差預(yù)測均值為

3.2 量測更新

以單個(gè)星敏感器為例生成觀測容積估計(jì)點(diǎn)為

通過以上容積點(diǎn)得到量測容積點(diǎn)均值：

求取濾波增益得：

其中協(xié)方差陣和互協(xié)方差陣分別表示如下：

再由式(50)可得誤差修正羅德里格參數(shù)和陀螺漂移的量測更新為

進(jìn)一步，得到容積點(diǎn)四元數(shù)：

協(xié)方差陣更新為

4 仿真分析

通過估計(jì)近地軌道航天器的姿態(tài)來驗(yàn)證SCI融合的有效性。假設(shè)航天器本體坐標(biāo)系與軌道坐標(biāo)系重合，。并假設(shè)航天器配有三個(gè)相同的星敏感器。角速度和角度矢量分別來自三軸陀螺儀和星敏感器的輸出。星敏感器測量帶有標(biāo)差為2″的高斯白噪聲。陀螺和星敏感器的采樣周期為1 s，仿真時(shí)長是8 h。假設(shè)初始角度估計(jì)誤差較大，。這里假定三軸初始角度估計(jì)誤差分別為45°、45°、155°，陀螺漂移初始估計(jì)誤差為零，。方差陣初始值每個(gè)星敏感器運(yùn)行各自的容積卡爾曼濾波器和一個(gè)共用的陀螺儀，如圖1所示。下面分別給出魯棒CI融合和SCI融合的姿態(tài)估計(jì)結(jié)果。

4.1 魯棒CI融合估計(jì)結(jié)果

為便于理解，圖4和圖6分別給出的是角度估計(jì)誤。差和陀螺漂移估計(jì)的2-范數(shù)曲線，圖5給出的是三軸角度估計(jì)誤差隨時(shí)間的變化曲線，圖7給出的是陀螺漂移估計(jì)誤差隨時(shí)間的變化曲線。在仿真中可以看出，。在容積卡爾曼濾波框架下采用魯棒CI融合估計(jì)得到的角度估計(jì)值和陀螺漂移估計(jì)值都有較好的估計(jì)精度，角度估計(jì)精度達(dá)到了0.002°以內(nèi)，陀螺漂移估計(jì)精度達(dá)到了0.01 (°)/h以內(nèi)，并且具有很好的收斂速度。

圖4 角度估計(jì)誤差2-范數(shù)Fig.4 2 - norm curve of angle estimation error

圖5 三軸角度估計(jì)誤差Fig.5 Three-axis angle estimation errors

圖6 陀螺漂移估計(jì)誤差2-范數(shù)Fig.6 2 - norm curve of gyro drift estimate error

圖7 陀螺漂移估計(jì)誤差Fig.7 Estimate error of gyro drift

4.2 SCI融合估計(jì)結(jié)果

應(yīng)用第2節(jié)給出的SCI融合算法對三個(gè)姿態(tài)跟蹤系統(tǒng)進(jìn)行融合處理，并給出了融合結(jié)果。圖8和圖10分別給出的是角度估計(jì)誤差和陀螺漂移估計(jì)的2-范數(shù)曲線，圖9給出的是三軸角度估計(jì)誤差隨時(shí)間的變化曲線，圖11給出的是陀螺漂移估計(jì)誤差隨時(shí)間變化曲線。

與魯棒CI融合估計(jì)的仿真對比發(fā)現(xiàn)，SCI融合后的角度估計(jì)精度和陀螺漂移估計(jì)精度都有所提高，其中陀螺漂移估計(jì)精度提高較為明顯，角度估計(jì)精度提高了10%左右，陀螺估計(jì)精度提高了20%左右。從仿真結(jié)果可知，SCI融合估計(jì)的收斂速度要明顯高于魯棒CI融合估計(jì)。

圖8 角度估計(jì)誤差2-范數(shù)Fig.8 2 - norm curve of angle estimation error

圖9 三軸角度估計(jì)誤差Fig.9 Three-axis angle estimation error

圖10 陀螺漂移估計(jì)誤差2-范數(shù)Fig.10 2 - norm curve of gyro drift estimation error

圖11 陀螺漂移估計(jì)誤差Fig.11 Gyro drift estimation error

5 結(jié) 論

本文給出了一種高效的CI融合算法。先將每組傳感器所對應(yīng)的局部估計(jì)系統(tǒng)建模成一個(gè)子系統(tǒng)，在融合過程中利用CKF濾波器得到各子系統(tǒng)的一個(gè)局部最優(yōu)估計(jì)，然后使用矩陣加權(quán)線性最小方差最優(yōu)融合準(zhǔn)則獲取最優(yōu)融合估計(jì)。局部估計(jì)框架的狀態(tài)描述采用誤差修正羅德里格斯參數(shù)和誤差四元數(shù)相互切換來避免四元數(shù)規(guī)范約束和奇異的發(fā)生。此外，本文提出的SCI融合算法具有良好的計(jì)算效率。把多維非線性代價(jià)函數(shù)的優(yōu)化問題減少到對幾個(gè)一維非線性代價(jià)函數(shù)的優(yōu)化。通過與魯棒CI融合算法進(jìn)行仿真對比，。驗(yàn)證了本文融合方法的有效性和優(yōu)越性。

（References）

[1] Uhlmann, J K. General data fusion for estimates with unknown cross covariances[C]//Proceedings of the SPIE. Orlando, FL, 1996: 536-547.

[2] Julier S J, Uhlmann J K. A non-divergent estimation algorithm in the presence of unknown correlations[C]// Proceedings of American Control Conference. Vol.4, Albuquerque, NM, June 1997: 2369-2373.

[3] Crassidis J L, Cheng Y, Nebelecky C K, Fosbury A M. Decentralized attitude estimation using a quaternion covariance intersection approach[J]. Journal of the Astronautical Sciences, 2009, 57(1-2): 113-128.

[4] Niehsen W, Gmbh R B. Information fusion based on fast covariance intersection filtering[C]//Proceedings of the 5th International Conference on Information Fusion. 2002: 901-905.

[5] Nebelecky C K, Crassidis J L, Banas W D, Cheng Y, Fosbury A M. Decentralized relative attitude estimation for three-spacecraft formation flying applications[C]// AIAA Guidance, Navigation and Control Conference. Chicago, IL, August 2009.

[6] Zhou Jiang-hua, Miao Yu-hong. A Rodrigues-vectorbased strapdown attitude algorithm[C]//IEEE Aerospace Conference. 2012: 439-517.

[7] Nebelecky, Christopher K. Attitude data fusion using a modified Rodrigues parametrization[C]//AIAA Guidance, Navigation, and Control Conference. 2010: 917-933.

[8] Keefe O, Stephen A, Hanspeter S. Shadow set considerations for modified Rodrigues parameter attitude filtering [C]//Conference Article. August 2013: 2777-2786.

[9] Wahba G. Problem 65-1: A least squares estimate of satellite attitude[J]. SIAM Review, July 1965, 7(3): 409.

[10] Karlgaard C D, Schaub H. Nonsingular attitude filtering using modified rodrigues parameters[J]. Journal of the Astronautical Sciences, 2009: 777-791.

[11] Han, Ping1, Sang WeiLin. Simplex unscented Kalman filter for aircraft attitude algorithm[J]. Journal Article, 2014: 629-633.

[12] Zhao X.M, Yang L, Hui F. Three-dimensional vehicle attitude estimation using modified invasive weed optimized particle filter[J]. International Journal of Automotive Technology, 2014: 1143-1154.

[13] 王思思, 齊國清. 有色量測噪聲下的改進(jìn)求容積卡爾曼濾波器[J]. 控制理論與應(yīng)用, 2015, 32(1): 1-10. Wang Si-Si, Qi Guo-qing. Improved cubature Kalman filter with colored measurement noise[J]. Control Theory and Applications, 2015, 32(1): 1-10.

[14] Roh K, Park S, Park E, Choi K. A batch filter based on the unscented transformation and its applications to attitude determination[J]. Advances in Astronautical Sciences, 2007, 127(1): 19-40.

[15] Cheng Y, Crassidis J L. Particle filtering for attitude estimation using a minimum local-error representation [J]. Journal of Guidance, Control, and Dynamics, 2010, 33(4): 1305-1310.

[16] 王碩, 宋申民, 史小平, 于浛. 噪聲特性未知的多傳感器協(xié)方差交叉融合姿態(tài)估計(jì)[J]. 控制與決策, 2014. Wang Shuo, Song Shen-min, Shi Xiao-ping, Yu Han. The multi-sensor covariance intersection fusion attitude estimation with unknown noise characteristics[J]. Control and Decision, 2014.

[17] Julier S J, Uhlman J K. Non-divergent estimation algorithm in the presence of unknown correlations[C]// Proceedings of the IEEE American Control Conference, Albuquerque, NM, USA, 1997: 2369-2373

[18] Crassidis J L, Landis Markley F, Cheng Y. Survey of nonlinear attitude estimation methods[J]. Journal of Guidance, Control and Dynamics, 2007, 30(1): 12-28.

[19] Crassidis J, Junkins J. Optimal estimation of dynamic systems[M]. Boca Raton: Chapman & Hall, 2004: 419- 433.

Attitude fusion approach for multisensor with unknown noise characteristics

WANG Shuo, SONG Shen-min, YU Han
(Center for Control Theory and Guidance Technology, Harbin Institute of Technology, Harbin 150001, China)

A covariance intersection fusion estimation algorithm is presented for multi-sensor system with unknown covariance and noise characteristics, which means that the relativity and variance of the state noise and measurement noise are not available. Firstly, a corresponding CKF estimator is chosen for each subsystem to produce a local estimation according to the measurement data newly acquired. Secondly, based on the least matrix-weighted linear variance rule, a fast successive covariance intersection (SCI) fusion algorithm is proposed to achieve the optimal fusion estimation, which simplifies the multidimensional optimization problem into the optimization of several one-dimensional nonlinear cost functions. Thirdly, in the subsystems, a method of switching between error quaternion and error modified rodrigues parameter is used to avoid the norm constraint of quaternion and the singular problem of modified rodrigues parameter. Finally, numerical simulations are presented to demonstrate the effectiveness of the proposed algorithm.

cubature Kalman filter estimate; information fusion; covariance intersection; quaternion; rodrigues parameter

V448.2

：A

2015-06-15；

：2015-09-28

國家自然科學(xué)基金（61174037）；國家自然科學(xué)基金創(chuàng)新群體項(xiàng)目（61021002）

王碩（1984—），男，博士研究生。E-mail：wangshuo_hit@163.com

聯(lián) 系 人：宋申民（1968—），男，教授，博士生導(dǎo)師，研究方向?yàn)榉蔷€性系統(tǒng)的穩(wěn)定性分析、魯棒控制、導(dǎo)彈制導(dǎo)與飛行器控制。E-mail：songshenmin@hit.edu.cn

1005-6734(2015)05-0653-09

10.13695/j.cnki.12-1222/o3.2015.05.017