[發(fā)明專利]一種RTK用自適應(yīng)漸消Kalman濾波算法

有效

• 申請?zhí)枺?/td>: 201711143734.5
• 申請日: 2017-11-17
• 公開（公告）號: CN108226976B
• 公開（公告）日: 2021-10-19
• 發(fā)明（設(shè)計(jì)）人: 高亞豪;左啟耀;李峰;胡文濤;袁曉宇;王強(qiáng);王勛;李建華;鄒志勤;姜福灝
• 申請（專利權(quán)）人: 北京自動化控制設(shè)備研究所
• 主分類號: G01S19/44
• 分類號: G01S19/44
• 代理公司: 核工業(yè)專利中心 11007
• 代理人: 張雅丁

1.一種RTK用自適應(yīng)漸消Kalman濾波算法，其特征在于，包括如下步驟：

(1)衛(wèi)星捕獲跟蹤與同步

基準(zhǔn)站和流動站依次捕獲衛(wèi)星導(dǎo)航信號，對跟蹤衛(wèi)星進(jìn)行位同步和幀同步；

(2)觀測量提取

提取幀同步后衛(wèi)星觀測量，并獲得完成幀同步后的各顆導(dǎo)航衛(wèi)星的星歷參數(shù)；

(3)確定衛(wèi)星位置速度計(jì)算

輸入已知的衛(wèi)星發(fā)射時刻和步驟(2)中得到的星歷參數(shù)計(jì)算得到衛(wèi)星位置；

(4)建立Kalman濾波模型

設(shè)待估計(jì)的狀態(tài)向量x為

x＝(r,v,N₁,N₂)^T (1)

其中：

r＝(x,y,z)為接收機(jī)的位置參數(shù)；

v＝(v_x,v_y,v_z)為接收機(jī)的速度參數(shù)；

為所有衛(wèi)星的站間單差整周模糊度參數(shù)，下標(biāo)i代表載波L_i對應(yīng)的載波相位；

該Kalman濾波模型的動態(tài)模型采用等速模型，對應(yīng)接收機(jī)位置和速度參數(shù)的狀態(tài)轉(zhuǎn)移函數(shù)是一個線性函數(shù)F_r，用矩陣的形式表示為

其中，I為3×3的單位陣，t_s為兩個觀測歷元之間的時間間隔；

對于模糊度參數(shù)，當(dāng)衛(wèi)星信號跟蹤正常時，其對應(yīng)的模糊度固定不變；

當(dāng)發(fā)生周跳或失鎖時，為保證對應(yīng)的模糊度能夠快速收斂，對模糊度參數(shù)重新賦值，并更新協(xié)方差矩陣中的相應(yīng)元素；這一狀態(tài)轉(zhuǎn)移過程是非線性的；

系統(tǒng)的過程噪聲協(xié)方差矩陣設(shè)為對角陣，其對角線元素的大小根據(jù)實(shí)際載體的運(yùn)動狀態(tài)確定；

設(shè)系統(tǒng)的觀測向量y為

y＝(φ₁,φ₂,p₁,p₂)^T (3)

其中，為兩個頻率的雙差載波相位觀測值；

為兩個頻率的雙差偽距觀測值；

根據(jù)雙差載波相位和偽距觀測方程，得觀測函數(shù)為：

其中，為對應(yīng)載波相位觀測量的觀測函數(shù)；

為對應(yīng)偽距觀測量的觀測函數(shù)；

其中，為基準(zhǔn)站b、流動站r對衛(wèi)星m和衛(wèi)星n的雙差距離；

λ_i為載波L_i的波長；

和分別為衛(wèi)星m和衛(wèi)星n的雙差載波相位和雙差偽距的觀測噪聲；

其對應(yīng)的觀測噪聲協(xié)方差矩陣R為

其中，

和分別為載波相位和偽距觀測量的標(biāo)準(zhǔn)偏差；

為由單差觀測量向雙差觀測量轉(zhuǎn)換的單差算子；

(5)新息協(xié)方差矩陣估計(jì)

根據(jù)k時刻的觀測值y_k和一步預(yù)測值定義新息序列為：

假定新息的期望為零，因此新息的實(shí)際協(xié)方差矩陣的估計(jì)式為

當(dāng)時，說明系統(tǒng)的動態(tài)模型與實(shí)際運(yùn)動狀態(tài)出現(xiàn)偏差，導(dǎo)致實(shí)際的新息協(xié)方差陣與其理論計(jì)算值不符，這時自適應(yīng)地調(diào)整預(yù)測誤差協(xié)方差矩陣；其中，trace(·)是矩陣跡的符號；

如式(7)所示，要計(jì)算當(dāng)前時刻的新息協(xié)方差需要利用之前k個時刻的新息；

為了保證計(jì)算的準(zhǔn)確性，這些新息的維數(shù)必須相同，且同一個新息向量z_k中的元素必須對應(yīng)相同的歷元；

在差分定位中，每一顆衛(wèi)星的觀測量都可以間接反映流動站載體的位置和速度，因此利用部分觀測量新息計(jì)算出的遺忘因子λ_k反映載體的動態(tài)變化；

采用獨(dú)立滑動窗的方式收集每顆衛(wèi)星的新息；收集新息并計(jì)算其協(xié)方差矩陣的方法為：

a)確定滑動窗總長度N；對于每顆衛(wèi)星的每種觀測量都建立一個滑動窗；

b)計(jì)算當(dāng)前歷元各顆衛(wèi)星的觀測量對應(yīng)的新息；若相對于上一歷元參考星發(fā)生變化，則根據(jù)雙差運(yùn)算的定義對所有滑動窗進(jìn)行新息的轉(zhuǎn)換；

c)若當(dāng)前時刻某觀測量的新息z_k對應(yīng)的滑動窗長度n＜N，則z_k直接添加到滑動窗中，滑動窗長度n增加1；若n＝N，則用z_k替換最早的新息z_k-N；若當(dāng)前歷元沒有獲得對應(yīng)的新息，則將滑動窗內(nèi)的數(shù)據(jù)全部清除，即n＝0；

d)選擇所有滿足n＝N的滑動窗，然后根據(jù)式(7)計(jì)算新息的協(xié)方差矩陣(6)自適應(yīng)漸消調(diào)整

濾波模型建立之后，按照下面公式進(jìn)行實(shí)時參數(shù)估計(jì)：

其中，λ_k為遺忘因子，且λ_k≥1；

A_k-1和H_k分別表示狀態(tài)轉(zhuǎn)移函數(shù)和觀測函數(shù)線性化后對應(yīng)的雅克比矩陣；

和P_k分別表示預(yù)測誤差協(xié)方差陣和濾波誤差協(xié)方差陣；

u_k-1表示Kalman濾波器的輸入，在該模型中為0；

Q_k-1表示系統(tǒng)的過程噪聲協(xié)方差矩陣；

R_k表示觀測噪聲協(xié)方差矩陣；

K_k表示歷元的增益矩陣；

W_k-1表示k-1歷元的系統(tǒng)測量噪聲權(quán)矩陣的轉(zhuǎn)置；

運(yùn)算符(·)^T表示對矩陣求轉(zhuǎn)置；

I表示n×n的單位陣，其中n表示狀態(tài)向量中元素的個數(shù)；

當(dāng)系統(tǒng)模型與輸入觀測值不匹配時，將一步預(yù)測誤差協(xié)方差擴(kuò)大λ_k倍，使當(dāng)前觀測數(shù)據(jù)在狀態(tài)估計(jì)中的權(quán)重增大，從而避免了濾波器的發(fā)散；

按照下式計(jì)算遺忘因子λ_k：

設(shè)定濾波代入的過程噪聲協(xié)方差足夠小至忽略，得到遺忘因子

其中，為估計(jì)得到的新息協(xié)方差矩陣。