[發明專利]基于Cholesky分解計算的精確擴展Stirling插值濾波方法

有效

• 申請號: 202010893998.8
• 申請日: 2020-08-31
• 公開（公告）號: CN111998854B
• 公開（公告）日: 2022-04-15
• 發明（設計）人: 丁國強;劉娜;焦玉召;趙素娜;王曉雷;凌丹;王妍;王延峰
• 申請（專利權）人: 鄭州輕工業大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C25/00;G06F17/15;G06F17/16
• 代理公司: 鄭州優盾知識產權代理有限公司 41125
• 代理人: 張真真

本發明提出了一種基于Cholesky分解計算的精確擴展Stirling插值濾波方法，用于實現SLAM系統狀態空間模型狀態參數最優濾波計算，屬于導航定位與控制領域。本發明基于SLAM狀態空間模型非線性動態方程與離散化觀測數據，利用Stirling插值多項式逼近計算獲得SLAM系統等價模型方程，根據離散化觀測數據的采樣區間確定非線性系統方程Stirling插值多項式的精確積分計算；針對傳統NIRK積分開展局部和全局誤差控制計算，將其數值積分計算過程融入到二階Stirling插值多項式一階均值和二階方差逼近計算中來實現新型二階Stirling濾波算法設計過程。經由SLAM系統仿真，并與傳統二階擴展Stirling插值濾波算法對比，驗證本發明算法的計算優勢和計算效能。

技術領域

本發明涉及機器人系統技術領域的導航定位授時(Positioning、Naviagtion andTiming,PNT)服務中的機器人系統信息處理技術領域，具體涉及一種基于Cholesky分解計算的精確擴展Stirling插值濾波方法。

背景技術

濾波技術是研究如何從受干擾的信號觀測結果中準確估計出未知的真實信號或者系統狀態變量參數的一門技術，它依據一定的估計準則，按照某種估計方法實現對信號的準確濾波計算，不同的估計準則、不同的觀測序列數據、和觀測信號方式會導致估計方法的差異，因此濾波技術經歷了最小二乘理論、Wiener濾波理論、Kalman濾波理論以及現代的非線性濾波理論算法發展而不斷完善與提高。傳統Bayesian隨機概率濾波理論要求已知過程噪聲和觀測噪聲的統計特性，或者假設其滿足一定的概率分布條件，而實際的非線性系統中系統狀態或者參數的統計特性往往是非高斯、非線性的，因此常規的非線性隨機概率濾波算法的應用有很大的局限性。如擴展Kalman濾波算法的高階截斷誤差影響，中心差分濾波算法的高階項截尾影響等，那么尋求一種從系統觀測量中在線實時最優估計動態系統狀態變量參數的最優濾波計算是科技人員面臨的重大挑戰。

對于非線性狀態空間模型中的非線性系統方程和觀測方程式，基于Taylor級數線性化處理得到的擴展集員濾波算法存在著很大的缺陷，首先當系統非線性比較強時，圍繞系統狀態參數預測估計或者狀態參數預估值的一階Taylor級數展開式往往存在著很大的截斷誤差，使得該算法存在數值計算穩定性變差，計算復雜，甚至出現濾波算法發散現象；再者一階Taylor級數擴展需要計算Jacobi矩陣，二階Taylor級數擴展需要計算復雜的Hessian矩陣，計算量巨大，對處理器要求很高，難以滿足實際系統快速計算要求。

發明內容

針對上述背景技術中存在的不足，本發明提出了一種基于Cholesky分解計算的精確擴展Stirling插值濾波方法，解決了Taylor級數擴展表達式計算的復雜度高，擴展Kalman濾波算法的計算精度差的技術問題。

本發明的技術方案是這樣實現的：

一種基于Cholesky分解計算的精確擴展Stirling插值濾波方法，其步驟如下：

步驟一、構建機器人SLAM系統連續-離散混合狀態空間模型，并設置機器人SLAM系統的狀態變量初值特性數據；

步驟二、根據狀態空間模型以及機器人SLAM系統的狀態變量初值計算第t_k-1時刻的系統狀態變量的估計值和估計方差矩陣P_k-1，并對系統狀態變量的估計方差矩陣進行J-正交Cholesky分解操作，得到其中，S_k-1表示估計方差矩陣的平方根；

步驟三、利用Stirling插值多項式對系統狀態變量估計值進行線性化后預測t_k時刻的系統狀態變量的預測值和預測方差矩陣P_k,k-1；

步驟四、在離散化觀測采樣區間內利用簡化牛頓迭代法對t_k時刻的系統狀態變量的預測值進行迭代更新，并計算每次迭代區間的中點方差矩陣；