余 航，王 堅(jiān)，王樂洋，寧一鵬，劉志平

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

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

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

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

1.1 動(dòng)態(tài)EIV數(shù)學(xué)模型形式

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

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

(1)

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

(2)

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

(3)

假設(shè)式(1)—式(3)中所有誤差項(xiàng)在任意歷元均互不相關(guān)，則動(dòng)態(tài)EIV模型的隨機(jī)模型為

(4)

(5)

(6)

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

預(yù)測(cè)階段，先對(duì)式(1)變換如下[14]

(7)

考慮矩陣運(yùn)算

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個(gè)mm×m子矩陣。

因此，式(15)可推導(dǎo)得

(18)

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

(19)

式(19)等價(jià)于

(20)

(21)

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

(22)

上式等價(jià)于

(23)

結(jié)合式(13)與式(23)推導(dǎo)得

(24)

因此，一步預(yù)測(cè)值為

(25)

由式(1)、式(3)及式(25)，計(jì)算一步預(yù)測(cè)殘差

(26)

(27)

進(jìn)而有

(28)

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

(29)

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

(30)

(31)

(32)

(33)

與預(yù)測(cè)部分的計(jì)算思路類似，可推導(dǎo)得到

(34)

式(34)等價(jià)于

(35)

(36)

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

(37)

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

(38)

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

(39)

(40)

采用條件平差法[14]得到相應(yīng)誤差改正項(xiàng)為

(41)

(42)

(43)

(44)

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

(45)

(46)

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

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

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

(5) 計(jì)算：

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

2 算例與分析

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

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

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

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

(47)

(48)

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

(49)

初始?xì)v元的先驗(yàn)信息如下

Σ0=10-5·I7×7

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

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

Qφk=BQφkBT

(50)

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

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

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

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

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

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

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

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

(51)

(52)

通過對(duì)式(52)進(jìn)行適當(dāng)數(shù)學(xué)變換得

(53)

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

(54)

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

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

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

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

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

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

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

表1本文方法與已有TKF和WTLS平差方法的對(duì)比結(jié)果

Tab.1ComparisonoftheproposedmethodwiththeexistingTKFandWTLSmethods

情況對(duì)比結(jié)果1等價(jià)于文獻(xiàn)[21]方法2等價(jià)于文獻(xiàn)[19]方法3等價(jià)于文獻(xiàn)[8]中方法64等價(jià)于文獻(xiàn)[8]中方法3及文獻(xiàn)[14]方法

3 結(jié) 論

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

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

參考文獻(xiàn)：

[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] 楊元喜. 自適應(yīng)動(dòng)態(tài)導(dǎo)航定位[M]. 2版. 北京: 測(cè)繪出版社, 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)合平差坐標(biāo)轉(zhuǎn)換模型的等價(jià)性[J]. 武漢測(cè)繪學(xué)院學(xué)報(bào), 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)南, 劉大杰. 大地坐標(biāo)和地心坐標(biāo)精度對(duì)聯(lián)合平差的精度影響[J]. 測(cè)繪學(xué)報(bào), 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)合平差的理論和應(yīng)用[J]. 武漢測(cè)繪科技大學(xué)學(xué)報(bào), 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平差模型及其加權(quán)整體最小二乘估計(jì)[J]. 測(cè)繪學(xué)報(bào), 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]. 測(cè)繪學(xué)報(bào), 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] 王樂洋, 余航, 李毅. 一種加權(quán)總體最小二乘問題的解法[J]. 中國(guó)礦業(yè)大學(xué)學(xué)報(bào), 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] 崔希璋, 於宗儔, 陶本藻, 等. 廣義測(cè)量平差[M]. 2版. 武漢: 武漢大學(xué)出版社, 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.