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

有效

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

1.一種基于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時刻的系統狀態變量的預測值進行迭代更新，并計算每次迭代區間的中點方差矩陣；

步驟五、根據迭代后的系統狀態變量的預測值更新t_k時刻的觀測值，并在系統狀態變量的預測值處計算t_k時刻的偽觀測矩陣；

步驟六、根據偽觀測矩陣及其對應的觀測噪聲方差矩陣將中點方差矩陣進行下三角矩陣變換，并根據下三角矩陣變換結果計算t_k時刻的系統狀態變量的估計值和估計方差矩陣。

2.根據權利要求1所述的基于Cholesky分解計算的精確擴展Stirling插值濾波方法，其特征在于，所述機器人SLAM系統連續-離散混合狀態空間模型為：

其中，x_k表示t_k時刻的狀態變量集合，x_k∈Rⁿ表示t_k時刻的狀態變量，z_k∈R^m表示t_k時刻的觀測向量，f(·)和h(·)均是非線性二階可導函數，q(t)∈Rⁿ表示隨時間變化的過程噪聲，r_k∈R^m表示隨時間變化的觀測噪聲，G(t)表示n×q的噪聲方差矩陣，x(t)表示連續型系統狀態變量；

因此，機器人SLAM系統的初始狀態x₀屬于一個已知集合x₀∈X₀，且系統初始狀態滿足統計特性其中，表示初始狀態變量的估計值，Π₀表示系統初始狀態方差矩陣，且S₀為系統初始狀態方差矩陣的平方根。

3.根據權利要求1所述的基于Cholesky分解計算的精確擴展Stirling插值濾波方法，其特征在于，所述利用Stirling插值多項式對系統狀態變量估計值進行線性化的操作為：

其中，x_k表示t_k時刻的狀態變量，f(·)是非線性二階可導函數，D_△x項稱為差分算子；

其中，表示第k-1時刻的系統狀態變量的估計偏差，μ_p為偏差算子，δ_p為平均算子；

所述偏差算子μ_p為：

所述平均算子δ_p為：

其中，為沿軸向的單位向量，Δx_p表示系統狀態變量解耦后的估計偏差量，s為插值步長；

所述t_k時刻的系統狀態變量的預測值為：

其中，n表示系統狀態變量維數；

預測方差矩陣P_k,k-1為：

其中，