余 航，王 堅，王樂洋，寧一鵬，劉志平

1. 中國礦業(yè)大學環(huán)境與測繪學院，江蘇 徐州 221116； 2. 北京建筑大學測繪與城市空間信息學院，北京100044； 3. 東華理工大學測繪工程學院，江西 南昌 330013

卡爾曼濾波(Kalman filter，KF)方法已在組合導航、GPS定位及目標跟蹤等方面取得了廣泛應用，是一種處理動態(tài)模型獲得時變參數(shù)的經(jīng)典方法[1]。常用的卡爾曼濾波方法是基于最小二乘估計或最小方差估計的標準卡爾曼濾波方法[2]。此后，多種擴展方法相繼提出，如抗差卡爾曼濾波、抗差自適應卡爾曼濾波、約束卡爾曼濾波，又如擴展卡爾曼濾波、無跡卡爾曼濾波、粒子濾波等[3]。然而，在某些導航應用中，動態(tài)模型中觀測方程的系數(shù)矩陣及狀態(tài)方程的狀態(tài)轉移矩陣均存在誤差影響，嚴格意義上應考慮采用總體最小二乘(total least squares，TLS)平差方法。對于非動態(tài)系統(tǒng)，文獻[4—6]首次將總體最小二乘方法的思想應用于求解大地測量領域中的坐標轉換問題，此后許多學者對其進行了廣泛研究[7-17]。對于動態(tài)系統(tǒng)，文獻[18]同時顧及了動態(tài)系統(tǒng)的輸入與輸出項誤差，將總體最小二乘方法轉化為無約束非線性規(guī)劃問題并用于航跡的微分平滑系統(tǒng)。由于絕大多數(shù)動態(tài)模型均呈現(xiàn)出非線性的特點，但變量間的非線性關系往往可通過線性化、變量變換等轉化為線性關系，因此分析線性情況下的動態(tài)模型最為普遍。文獻[19]在線性動態(tài)系統(tǒng)的條件下，首次提出的動態(tài)EIV(errors-in-variables，EIV)模型的概念，提出將總體最小二乘方法應用于求解動態(tài)EIV模型并謂之總體卡爾曼濾波(total Kalman filter，TKF)方法，但在推導過程中僅視觀測方程的系數(shù)矩陣存在誤差，且系數(shù)矩陣誤差的方差-協(xié)方差陣須滿足特定的結構，當觀測方程存在粗差時，文獻[20]將粗差探測方法應用于TKF方法中。文獻[21]在文獻[19]的基礎上，去除了系數(shù)矩陣誤差的方差-協(xié)方差陣的結構限制，給出了更為一般方差陣情況下的WTKF(weighted total Kalman filter，WTKF)方法。

本文在已有總體卡爾曼濾波方法的基礎上，同時考慮了狀態(tài)轉移矩陣誤差與系數(shù)矩陣誤差的影響，給出了更為一般情況下的動態(tài)EIV模型。針對該模型的平差計算，采用虛擬觀測法分別構建了預測部分與修正部分的TLS平差準則[22]，推導了相應的總體卡爾曼濾波方法。以室內定位為背景，利用超寬帶(ultra-wideband，UWB)提供的測距信息與慣性導航系統(tǒng)(inertial navigation system，INS)輸出的角度信息和比力信息進行聯(lián)合處理[23-25]，將本文方法應用于確定室內載體的位置與姿態(tài)。算例結果表明了本文方法的有效性與可行性。

1 動態(tài)EIV模型及其總體卡爾曼濾波方法

1.1 動態(tài)EIV數(shù)學模型形式

在某一歷元k，動態(tài)EIV模型的函數(shù)模型形式為

ξk=(φk-Eφk)ξk-1+fk+wk

(1)

yk=(Ak-EAk)ξk+Zk+eyk

(2)

式中，ξk為m×1維時變隨機狀態(tài)向量；φk為m×m狀態(tài)轉移矩陣；Eφk為φk的隨機誤差矩陣；yk為n×1維觀測向量；Ak為n×m系數(shù)矩陣，其對應的隨機誤差矩陣為EAk；fk和Zk分別為狀態(tài)方程和觀測方程的控制向量；wk和eyk分別為m×1維隨機系統(tǒng)噪聲和n×1維觀測噪聲。

(3)

假設式(1)—式(3)中所有誤差項在任意歷元均互不相關，則動態(tài)EIV模型的隨機模型為

(4)

(5)

(6)

1.2 動態(tài)EIV模型的總體卡爾曼濾波方法

預測階段，先對式(1)變換如下[14]

(7)

考慮矩陣運算

vec(ABC)=(CT?A)·vec(B)

(8)

式中，?表示Kronecker積。

(9)

(10)

(11)

(12)

由式(11)可得如下等式成立

(13)

由式(12)可得

(14)

(15)

式中，ξk-1=[(ξk-1)1… (ξk-1)i… (ξk-1)m]T，i∈[1,m]。

式(15)中[8,14]

(16)

(17)

式中，Qφk=[(Qφk)1… (Qφk)i… (Qφk)m]；(Qφk)i為Qφk中的第i個mm×m子矩陣。

因此，式(15)可推導得

(18)

將式(14)和式(18)代入式(12)，得到式(12)的具體表達式為

(19)

式(19)等價于

(20)

(21)

將式(21)代入式(20)得

(22)

上式等價于

(23)

結合式(13)與式(23)推導得

(24)

因此，一步預測值為

(25)

由式(1)、式(3)及式(25)，計算一步預測殘差

(26)

(27)

進而有

(28)

對式(28)采用協(xié)方差傳播率，可得一步預測的方差-協(xié)方差陣為

(29)

式(25)與式(29)為修正計算提供了驗前信息。在進行修正部分之前，同理對式(2)變換

(30)

(31)

(32)

(33)

與預測部分的計算思路類似，可推導得到

(34)

式(34)等價于

(35)

(36)

聯(lián)立式(30)和式(36)得法方程如下

(37)

結合矩陣反演公式(V+CZD)-1=V-1-V-1C(Z-1+DV-1C)-1DV-1，并計算上述法方程得

(38)

將式(38)的計算結果分別代入式(28)和式(30)，得

(39)

(40)

采用條件平差法[14]得到相應誤差改正項為

(41)

(42)

(43)

(44)

對式(44)采用協(xié)方差傳播率可得驗后狀態(tài)估值的方差-協(xié)方差陣為

(45)

(46)

1.3 動態(tài)EIV模型總體卡爾曼濾波方法的迭代計算過程

動態(tài)EIV模型總體卡爾曼濾波方法的迭代計算過程如下：

(2) 歷元k=k+1：輸入觀測數(shù)據(jù)φk、fk、Ak、yk、Zk、Qφk、θk和Qk。

(5) 計算：

(8) 若k≤t(t表示總的歷元數(shù))，重復步驟1-7，否則，迭代結束。

2 算例與分析

仿真試驗：模擬室內軌跡(如圖1所示)，載體利用UWB給出的距離觀測值與INS提供的角度及比力信息進行位置和載體姿態(tài)的確定。為了計算方便，設初始時刻載體系(xt,yt,zt)與導航系(xn,yn,zn)嚴格對齊(即初始姿態(tài)角為零)，導航系采用東北天坐標系；為了計算方便，文中將軌跡及UWB基站的坐標均融入自定義的局部坐標系中。

圖1 載體軌跡示意圖Fig.1 Sketch map of indoor carrier

誤差模擬：現(xiàn)假設INS的陀螺與加速度計的常值零偏為零；模擬INS器件的陀螺隨機游走誤差為0.5°/s，加速度計的隨機游走誤差10 mg，UWB距離測量值的隨機誤差為0.01 m；視基站定位的時間測量誤差為常值，可將由時間測量誤差引起的測距誤差作為系統(tǒng)誤差，其大小分別為0.4 m、0.4 m、0.6 m、0.6 m；基站的位置誤差為0.08 m。

對于姿態(tài)參數(shù)的計算，通常轉化為四元素的更新，以避免航向角為90°時不可解的情況。姿態(tài)四元素更新方程為[25]

(47)

(48)

由UWB提供的測距信息以及姿態(tài)四元素觀測值，建立觀測方程為

(49)

初始歷元的先驗信息如下

Σ0=10-5·I7×7

各歷元的系統(tǒng)噪聲定為

對于式(48)的狀態(tài)方程，令vec(φk)=h+Bφk[7]，其中h對應于φk中的常值元素，B對應于φk中的個隨機元素，則狀態(tài)轉移矩陣φk的方差-協(xié)方差陣為

Qφk=BQφkBT

(50)

對于式(49)的觀測方程，其系數(shù)矩陣與觀測向量的方差-協(xié)方差陣可由文獻[10]方法確定，觀測向量中的四元素的方差協(xié)方差陣設為diag([0.03 0.03 0.03 0.03])。

給真實軌跡與姿態(tài)模擬誤差，由式(48)、式(49)建立各歷元的狀態(tài)方程與觀測方程；采用標準Kalman濾波方法(KF法)、文獻[21]方法(WTKF法)與本文方法作對比試驗；隨機模擬一次誤差情況下，不同方法得到的各歷元三維位置及姿態(tài)參數(shù)結果如圖2，其中姿態(tài)結果由最終估計得到的四元素結果轉換而得。

圖2 各方法的參數(shù)估值Fig.2 The adjustment results of each method

為了更好地進行對比分析，對上述試驗模擬計算10 000次，并將各歷元載體位置和姿態(tài)的平差結果與真值求差并取絕對值，獲得平差結果與真值的絕對誤差，將10 000次各歷元平差參數(shù)的實際偏差取均值。三維位置及姿態(tài)的絕對誤差均值結果見圖3。

分析仿真算例的平差結果，可得如下結論：

(1) 由隨機模擬一次誤差所得結果圖2可知，無論是各歷元的三維位置估值或是姿態(tài)估值，3種方法均能在一定程度上反映真實軌跡與姿態(tài)的變化趨勢；然而，整體上從各歷元的估值結果來看，本文方法所得位置與姿態(tài)結果均優(yōu)于KF法與WTKF法。從圖2(a)可知，在位置估值方面，本文方法在北向和西向與KF法和WTKF法所得結果近似度較高；在天向上KF法與WTKF法的結果較差，而本文方法始終能保持較高的解算精度。從圖2(b)可知，3種方法的解算結果較好地反映了航向的變化，而在俯仰角和橫滾角方面，本文方法的估值結果較優(yōu)。

(2) 由10 000次試驗的均值結果(圖3)可知，本文方法估計的各歷元位置絕對誤差均值與姿態(tài)絕對誤差均值結果整體上優(yōu)于KF法與WTKF法，從統(tǒng)計的角度表明了本文方法的有效性；隨著歷元的增加，3種方法的位置與姿態(tài)誤差均存在變大的趨勢。雖然在某一特定歷元，本文方法的誤差較之KF法或TKF法要大，但從此歷元后所有歷元上分析，這并未在很大程度上影響到本文方法對后續(xù)歷元的解算，可見，由于本文方法同時顧及了狀態(tài)方程中狀態(tài)轉移矩陣與觀測方程中系數(shù)矩陣的誤差項，相對于只顧及觀測方程系數(shù)矩陣誤差的WTKF法有了一定的改善，平差結果的有效性表明了本文方法是可行的。

(3) 從上述分析可知，由于同時顧及狀態(tài)方程中狀態(tài)轉移矩陣與觀測方程中系數(shù)矩陣的誤差項，本文方法的數(shù)值計算結果優(yōu)于已有KF和WTKF的結果。由于文獻[19]的TKF方法中觀測方程系數(shù)矩陣的方差-協(xié)方差陣須滿足特定結構Im?Qyk，因此不適合本算例的數(shù)值計算。下面從公式推導的角度進行簡單說明。假設不考慮狀態(tài)方程中狀態(tài)轉移矩陣的誤差項及狀態(tài)方程和觀測方程中的控制向量，且QAk=Im?Qyk，QykAk=QAkyk=0[19]。顧及到符號表達的不同，由文獻[19]中式(47)可知，參數(shù)估值為

(51)

(52)

通過對式(52)進行適當數(shù)學變換得

(53)

由矩陣反演公式，式(53)中

(54)

在滿足上述假設條件下，結合式(52)—式(54)，不難得出式(51)與式(38)相等，也即本文方法參數(shù)估值式(38)等價于文獻[19]中參數(shù)估值式(47)。因此，TKF方法可視為本文方法在滿足某些假設情況下的特例。同理，若不顧及狀態(tài)方程中系數(shù)陣誤差的影響，不難證明本文方法等價于文獻[21]的WTKF方法。

(4) 為了更好地分析，假設狀態(tài)方程和觀測方程中的控制向量為零，在本文動態(tài)EIV模型的基礎上，分以下4種情況進行對比與歸納：

情況1：不考慮狀態(tài)轉移矩陣誤差項影響。

情況2：不考慮狀態(tài)轉移矩陣誤差項影響，且觀測方程的方差-協(xié)方差陣滿足特定結構。

情況3：不考慮狀態(tài)方程。

情況4：不考慮狀態(tài)方程及參數(shù)的先驗信息。

綜合上述分析，已有TKF須假設觀測方程系數(shù)矩陣滿足特定結構且未顧及狀態(tài)方程系數(shù)陣的誤差項，因而其應用范圍有限；WTKF方法較之TKF方法，雖然其可處理觀測方程中隨機元素為一般方差陣的情況，但由于未顧及狀態(tài)方程中狀態(tài)轉移矩陣的誤差，限制了該方法的應用；在某些應用場景中，觀測方程的系數(shù)陣和狀態(tài)方程中狀態(tài)轉移矩陣可同時存在誤差項影響，因此采用本文方法更為合理。TKF、WTKF與加權總體最小二乘平差結果可視為是本文方法在滿足某種條件下的特例，可見本文推導的總體卡爾曼濾波方法是計算狀態(tài)方程與觀測方程均為線性情況下更為通用的一種總體卡爾曼濾波方法。

表1本文方法與已有TKF和WTLS平差方法的對比結果

Tab.1ComparisonoftheproposedmethodwiththeexistingTKFandWTLSmethods

情況對比結果1等價于文獻[21]方法2等價于文獻[19]方法3等價于文獻[8]中方法64等價于文獻[8]中方法3及文獻[14]方法

3 結 論

本文在已有的動態(tài)EIV模型基礎上，考慮了狀態(tài)方程中狀態(tài)轉移矩陣的誤差項影響，推導了能夠同時顧及狀態(tài)方程中狀態(tài)轉移矩陣與觀測方程中系數(shù)矩陣誤差的總體卡爾曼濾波方法。本文從公式推導的角度證明了，當觀測方程系數(shù)矩陣的方差-協(xié)方差陣滿足特定結構，且不顧及狀態(tài)方程中狀態(tài)轉移矩陣的誤差情況下，本文方法等價于TKF方法；本文推導的參數(shù)估值公式具有與標準Kalman濾波公式相似的結構，平差結果簡單、易于理解；算例試驗驗證了本文方法較之WTKF方法與KF方法能取得更優(yōu)的平差結果，表明了本文方法可行性與有效性。然而本文推導方法僅適用于狀態(tài)方程與觀測方程均為線性的情況，在實際情況下，絕大多數(shù)動態(tài)模型均呈現(xiàn)非線性的特點，推導適用于非線性動態(tài)EIV模型的總體卡爾曼濾波方法將是下一步研究的重點。

圖3 各方法絕對誤差均值Fig.3 The average values of absolute error by each method

參考文獻：

[1] CHANG Guobin. Alternative Formulation of the Kalman Filter for Correlated Process and Observation Noise[J]. IET Science, Measurement & Technology, 2014, 8(5): 310-318.

[2] 楊元喜. 自適應動態(tài)導航定位[M]. 2版. 北京: 測繪出版社, 2017.

YANG Yuanxi. Adaptive Navigation and Kinematic Positioning[M]. 2nd ed. Beijing: Surveying and Mapping Press, 2017.

[3] CRASSIDIS J L, JUNKINS J L. Optimal Estimation of Dynamic Systems[M]. 2nd ed. Boca Raton, FL: CRC Press, 2011.

[4] 劉經(jīng)南. 衛(wèi)星網(wǎng)與地面網(wǎng)聯(lián)合平差坐標轉換模型的等價性[J]. 武漢測繪學院學報, 1983, 8(1): 37-50.

LIU Jingnan. The Equivalence of Mathematical Models for Coordinate Systems Transformation in the Adjustment for the Combination of Satellite and Terrestrial Network[J]. Journal of Wuhan Technical University of Surveying and Mapping, 1983, 8(1): 37-50.

[5] 劉經(jīng)南, 劉大杰. 大地坐標和地心坐標精度對聯(lián)合平差的精度影響[J]. 測繪學報, 1985, 14(2): 133-144.

LIU Jingnan, LIU Dajie. The Influence of the Accuracy in Geodetic and Geocentric Coordinates on the Accuracy in the Results of Simultaneous Adjustment[J]. Acta Geodaetica et Cartographica Sinica, 1985, 14(2): 133-144.

[6] 劉經(jīng)南, 劉大杰, 崔希璋. 衛(wèi)星網(wǎng)與地面網(wǎng)聯(lián)合平差的理論和應用[J]. 武漢測繪科技大學學報, 1987, 12(4): 1-9.

LIU Jingnan, LIU Dajie, CUI Xizhang. Theory and Application of the Combined Adjustment of Satellite and Terrestrial Network[J]. Journal of Wuhan Technical University of Surveying and Mapping, 1987, 12(4): 1-9.

[7] XU Peiliang, LIU Jingnan, SHI Chuang. Total Least Squares Adjustment in Partial Errors-in-variables Models: Algorithm and Statistical Analysis[J]. Journal of Geodesy, 2012, 86(8): 661-675.

[8] FANG Xing. Weighted Total Least Squares: Necessary and Sufficient Conditions, Fixed and Random Parameters[J]. Journal of Geodesy, 2013, 87(8): 733-749.

[9] 曾文憲, 方興, 劉經(jīng)南, 等. 通用EIV平差模型及其加權整體最小二乘估計[J]. 測繪學報, 2016, 45(8): 890-894, 903. DOI: 10.11947/j.AGCS.2016.20150156.

ZENG Wenxian, FANG Xing, LIU Jingnan, et al. Weighted Total Least Squares of Universal EIV Adjustment Model[J]. Acta Geodaetica et Cartographica Sinica, 2016, 45(8): 890-894, 903. DOI: 10.11947/j.AGCS.2016.20150156.

[10] JAZAERI S, AMIRI-SIMKOOEI A. Weighted Total Least Squares for Solving Non-linear Problem: GNSS Point Positioning[J]. Survey Review, 2015, 47(343): 265-271.

[11] FANG Xing. Weighted Total Least-squares with Constraints: A Universal Formula for Geodetic Symmetrical Transformations[J]. Journal of Geodesy, 2015, 89(5): 459-469.

[12] FANG Xing, WANG Jin, LI Bofeng, et al. On Total Least Squares for Quadratic form Estimation[J]. Studia Geophysica et Geodaetica, 2015, 59(3): 366-379.

[13] 王樂洋, 余航, 陳曉勇. Partial EIV模型的解法[J]. 測繪學報, 2016, 45(1): 22-29. DOI: 10.11947/j.AGCS.2016.20140560.

WANG Leyang, YU Hang, CHEN Xiaoyong. An Algorithm for Partial EIV Model[J]. Acta Geodaetica et Cartographica Sinica, 2016, 45(1): 22-29. DOI: 10.11947/j.AGCS.2016.20140560.

[14] 王樂洋, 余航, 李毅. 一種加權總體最小二乘問題的解法[J]. 中國礦業(yè)大學學報, 2016, 45(6): 1263-1270.

WANG Leyang, YU Hang, LI Yi. A Method for Weighted Total Least Squares Problem[J]. Journal of China University of Mining & Technology, 2016, 45(6): 1263-1270.

[15] FANG Xing, LI Bofeng, ALKHATIB H, et al. Bayesian Inference for the Errors-in-variables Model[J]. Studia Geophysica et Geodaetica, 2017, 61(1): 35-52.

[16] WANG Bin, LI Jiancheng, LIU Chao, et al. Generalized Total Least Squares Prediction Algorithm for Universal 3D Similarity Transformation[J]. Advances in Space Research, 2017, 59(3): 815-823.

[17] ZHOU Yongjun, GONG Jinghai, FANG Xing. Accurate Coupled Lines Fitting in an Errors-in-variables Framework[J]. Survey Review, 2017. DOI: 10.1080/00396265.2017.1281095.

[18] LIU Ji, MENDOZA S, LI Guang, et al. Efficient Total Least Squares State and Parameter Estimation for Differentially Flat Systems[C]∥Proceedings of 2016 American Control Conference. Boston, MA: IEEE, 2016: 5419-5424.

[19] SCHAFFRIN B, IZ H B. Towards Total Kalman Filtering for Mobile Mapping[C]∥Proceedings of the 5th International Symposium on Mobile Mapping Technology. Padua, Italy: ISPRS, 2007: 270-275.

[20] SCHAFFRIN B, UZUN S. Errors-in-variables for Mobile Mapping Algorithms in the Presence of Outliers[J]. Archives of Photogrammetry, Cartography and Remote Sensing, 2011, 22(3): 377-387.

[21] MAHBOUB V, SAADATSERESHT M, ARDALAN A A. A General Weighted Total Kalman Filter Algorithm with Numerical Evaluation[J]. Studia Geophysica et Geodaetica, 2017, 61(1): 19-34. DOI: 10.1007/s11200-016-0815-7.

[22] 崔希璋, 於宗儔, 陶本藻, 等. 廣義測量平差[M]. 2版. 武漢: 武漢大學出版社, 2009.

CUI Xizhang, YU Zongchou, TAO Benzao, et al. Generalized Surveying Adjustment[M]. 2nd ed. Wuhan: Wuhan University Press, 2009.

[23] LI Zengke, CHANG Guobin, GAO Jingxiang, et al. GPS/UWB/MEMS-IMU Tightly Coupled Navigation with Improved Robust Kalman Filter[J]. Advances in Space Research, 2016, 58(11): 2424-2434.

[24] FARRELL J. Aided Navigation: GPS with High Rate Sensors[M]. New York: McGraw-Hill, Inc., 2008.

[25] YUAN Xuebing, YU Shuai, ZHANG Shengzhi, et al. Quaternion-based Unscented Kalman Filter for Accurate Indoor Heading Estimation Using Wearable Multi-sensor System[J]. Sensors, 2015, 15(5): 10872-10890.