SINS/GNSS組合導航：組合導航設計（卡爾曼濾波）

SINS/GNSS組合導航將SINS輸出信息與GNSS信息進行組合，狀態量爲

卡爾曼濾波原理參照卡爾曼濾波（KF）與擴展卡爾曼濾波（EKF）講解與推導，核心就是五個遞推方程迭代遞推出系統狀態變量，SINS/GNSS組合導航的卡爾曼濾波原理如下。

0 鬆組合和緊組合

GNSS 定位的長期穩定性與 SINS 系統定位的短期精確性具有近乎完美的互補特性，將兩者進行組合可以顯著提高導航精度。目前，大多數 SINS/GNSS 組合導航系統是利用 Kalman 濾波器將兩者組合起來的。因此，組合系統的濾波器狀態、量測信息、實現方式以及系統校正方式和組合深度都會影響到 SINS/GNSS 組合系統的工作性能。GNSS 與 SINS 的組合導航主要有三種模式：鬆組合、緊組合、深組合。

鬆組合的組合模式較爲簡單。在 GNSS 正常工作時，將 SINS 與其位置、速度之差作爲量測信息送入卡爾曼濾波器，然後通過濾波器估計並校正 SINS 誤差。此組合模式具有簡單實用，便於實現，系統流程如下圖所示。

緊組合利用僞距、僞距率進行組合。僞距、僞距率爲 GPS 接收機給出的信息， SINS 無法直接給出，因此需要根據 SINS 輸出位置和速度信息，結合 GPS 接收的星曆信息，計算出相應於 SINS 的僞距、僞距率，然後將兩個系統僞距、僞距率的差值作爲組合濾波器的輸入。最後利用卡爾曼濾波器估計並反饋 SINS 誤差，同時還對接收機鐘差等參數進行估計。系統流程圖如下圖所示：

相對於鬆組合模式，緊組合最大優點是在可見星的個數少於 4 顆時也可以使用，而且理論和工程應用都表明其組合後的位置、速度精度也略高。深組合設計傳感器設計內容，尚在研究。本文以鬆組合爲例介紹SINS/GNSS組合系統。

1 狀態方程

以SINS的誤差模型建立狀態方程，參考SINS/GNSS組合導航：SINS誤差模型，狀態量爲18維：有位置誤差（經緯高） $\delta p = [\delta L,\delta \lambda ,\delta h]$ 、速度誤差（東北天向速度） $\delta v =[\delta v_E, \delta v_N, \delta v_U]$ 、姿態角誤差 $\phi=[\phi_E,\phi_N, \phi_U]$ 、陀螺儀常值漂移誤差 $\varepsilon^b = [\varepsilon _x^b,\varepsilon _y^b, \varepsilon _z^b]$ ，陀螺儀一階馬爾科夫漂移誤差 $\varepsilon^r= [\varepsilon _x^r,\varepsilon _y^r, \varepsilon _z^r]$ ，加速度計一階馬爾科夫漂移誤差 $\bigtriangledown ^b=[\bigtriangledown _x,\bigtriangledown _y,\bigtriangledown _z]$ ，上述連接推到了誤差模型，可以得到狀態方程如下：

$\dot{X}(t)=F(t)X(t)+G(t)W(t)$ （1-1）

其中F爲：

$F = \begin{bmatrix} M_{aa} & M_{av} & M_{ap} & -C_b^n & -C_b^n& 0_{3\times3}\\ M_{va} & M_{vv}& M_{vp} & 0_{3\times3} & 0_{3\times3} & C_b^n\\ 0_{3\times3} & M_{pv} & M_{pp} & 0_{3\times3} &0_{3\times3} & 0_{3\times3} \\ 0_{3\times3}& 0_{3\times3}& 0_{3\times3} &0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\ 0_{3\times3}& 0_{3\times3}& 0_{3\times3} &0_{3\times3} & a_{g} & 0_{3\times3}\\ 0_{3\times3} & 0_{3\times3}& 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & a_{f} \end{bmatrix}$ （1-2）

將加速度計和陀螺誤差漂移項建模爲馬爾科夫模型，該模型通常可滿足高端 IMU 器件建模的精度要求。而 MEMS-IMU 相關時間很長，因此相關漂移項又可近似爲隨機遊走，則系統狀態方程變爲:

$F = \begin{bmatrix} M_{aa} & M_{av} & M_{ap} & -C_b^n & 0_{3\times3}\\ M_{va} & M_{vv}& M_{vp} & 0_{3\times3} & C_b^n\\ 0_{3\times3} & M_{pv} & M_{pp} & 0_{3\times3} & 0_{3\times3} \\ 0_{3\times3}& 0_{3\times3}& 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \end{bmatrix}$ （1-4）

F矩陣是已知的。參考SINS/GNSS組合導航：SINS誤差模型中的慣性器件誤差，設W(t)爲白噪聲，6維，表示如下：

（1-5）

$w_b=[w_{bx},w_{by},w_{bz}]$ 爲三軸陀螺儀隨機漂移誤差，爲陀螺儀一階馬爾科夫過程漂移，爲加速度計一階馬爾科夫過程漂移。

噪聲矩陣G(t)如下：

$G(t)=\begin{bmatrix} -C_b^n & 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3} & 0_{3\times 3} &C_b^n \\ 0_{3\times 3} &0_{3\times 3} & 0_{3\times 3}\\ 0_{3\times 3} & I_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3} & 0_{3\times 3} & I_{3\times 3}\end{bmatrix}$ （1-6）

利用隨機線性連續系統和隨機線性離散系統的關係：

$\\ \Phi _{k+1|k}=I+F(t_k)\bigtriangleup T+o(\bigtriangleup T^2)\\ \Gamma _{k+1|k}=F(t_k)\bigtriangleup T+o(\bigtriangleup T^2)\\$ （1-7）

忽略二階高次項得到：

$\\ \Phi _{k+1|k}=I+F(t_k)\bigtriangleup T\\ \Gamma _{k+1|k}=(I+F(t_k)\bigtriangleup T/2)G\bigtriangleup T\\$ （1-8）

將連續狀態方程（1-1）離散化：

$X_{k+1}=\Phi _{k+1|k}X_k+\Gamma _{k+1|k} W_k$ （1-9）

至此得到了觀測方程。

2 觀測方程

GNSS/SINS 鬆組合系統選取 SINS 解算的位置、速度同 GNSS 接收機得到的位置、速度之差作爲觀測量，即：

$Z_{k+1}=\begin{bmatrix} p^n_{INS}-p^n_{GNSS} \\ v^n_{INS}-v^n_{GNSS} \end{bmatrix}=H_kX_k+V_k$ （2-1）

$V_k=[\eta _p^T,\eta_v^T]^T$ 爲高斯觀測白噪聲，H矩陣如下：

$H_k=\begin{bmatrix} I_{3\times 3} & 0_{3\times 3} & 0_{3\times 3} & 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3} & I_{3\times3} & 0_{3\times 3} & 0_{3\times 3} & 0_{3\times 3} \end{bmatrix}$ （2-2）

3 開環卡爾曼濾波模型

卡爾曼濾波原理參照卡爾曼濾波（KF）與擴展卡爾曼濾波（EKF）講解與推導。根據狀態方程（1-9）與觀測方程（2-1）可以得到卡爾曼濾波方程：

$\\ \overline{X_k} &= \Phi _{k+1|k}\hat{X}_{k-1}\\ \overline{P_k} & = \Phi _{k+1|k}\hat{P}_{k-1} \Phi _{k+1|k}^T + Q_k\\ K_k &= \overline{P}_kH_k^T(H_k\overline{P_k}H_k^T+R_k)^{-1}\\ \hat{X_k}&=\overline{X}_k+K_k(Z_k-H_k\overline{X_k})\\ \hat{P_k}&=(I-K_kH_k)\overline{P}_k$ （3-1）

4 閉環卡爾曼濾波模型

實際上，對於SINS/GNSS組合導航系統而言，必須要對導航系統進行反饋校正。原因是前述組合導航系統的數學模型是建立在誤差狀態爲小量的基礎上的，而SINS 的誤差隨時間積累迅速，如果不進行反饋校正，即使在短時間內，慣導誤差也不再是小量。此時，卡爾曼濾波器的模型出現誤差，濾波誤差增大，系統甚至無法工作。

導航系統的反饋校正，就是將組合卡爾曼濾波器估計的 SINS 參數誤差反饋到慣導系統內部，對誤差狀態進行校正，從而形成閉環卡爾曼濾波器，如本文第一幅圖所示的 SINS 誤差反饋校正環節。校正環節框圖簡化如下：

相比開環系統狀態方程（1-9），閉環系統狀態方程增加了控制項，結合測量方程可以表示如下：

$\\ X_{k+1}=\Phi _{k+1|k}X_k+B_kU_k+\Gamma _{k+1|k} W_k\\ Z_{k+1}=H_kX_k+V_k$ （4-1）

觀測方程不變，卡爾曼濾波方程如下：

$\\ \overline{X_k} &= \Phi _{k+1|k}\hat{X}_{k-1} + B_{k-1}U_{k-1}\\ \overline{P_k} & = \Phi _{k+1|k}\hat{P}_{k-1} \Phi _{k+1|k}^T + Q_k\\ K_k &= \overline{P}_kH_k^T(H_k\overline{P_k}H_k^T+R_k)^{-1}\\ \hat{X_k}&=\overline{X}_k+K_k(Z_k-H_k\overline{X_k})\\ \hat{P_k}&=(I-K_kH_k)\overline{P}_k$ （4-2）

相比開環系統，區別只是預測部分增加了控制項。根據現代控制理論中的分離定理：針對具有二次型性能指標的隨機動態系統的最優控制問題，首先可以不必考慮控制，而是在隨機干擾下儘量精確地求解出狀態估計（最優估計），其次將最優估計視爲已知量，專門求解最優控制規律。

針對（4-1）描述的的隨機離散系統的最優問題，設系統的二次型性能指標爲：

$J_N=\sum_{N}^{j=1}(X_j^TQ_j^0X_j+U_j^TR_j^0U_j)+X_N^TS^0X_N$ （4-3）

優化問題通常按照的期望爲0這一約束條件進行求解：

$minE(J_N)=min(\sum_{N}^{j=1}(X_j^TQ_j^0X_j+U_j^TR_j^0U_j)+X_N^TS^0X_N)$ （4-4）

應用動態規劃法求解得到：

$U^{*}_k=-A_kX_k$ （4-5）

爲控制矩陣滿足：

$A_k=[R_k^0+B_k^TP_{k+1}^0B_k]^{-1}B_k^TP_{k+1}^0\Phi _{k+1|k}$ （4-6）

在二次性能指標中令 $R_{k}^0=0$ ，不考慮控制能量得到：

$A_k=B_k^{-1}\Phi _{k+1|k}$ （4-7）

（4-6）代入（4-4）得到：

$U^{*}_k=-B_k^{-1}\Phi _{k+1|k}X_k$ （4-8）

用 $\hat{X}_{k}$ 來估計得到：

$U^{*}_k=-B_k^{-1}\Phi _{k+1|k}\hat{X}_k$ （4-9）

將（4-8）代入閉環卡爾曼方程（4-2）得到：

$\\ \overline{X_k} &= 0\\ \overline{P_k} & = \Phi _{k+1|k}\hat{P}_{k-1} \Phi _{k+1|k}^T + Q_k\\ K_k &= \overline{P}_kH_k^T(H_k\overline{P_k}H_k^T+R_k)^{-1}\\ \hat{X_k}&=K_kZ_k\\ \hat{P_k}&=(I-K_kH_k)\overline{P}_k$ （4-10）

SINS/GNSS組合導航：組合導航設計（卡爾曼濾波）

容器中nginx無法使用同一個網絡下的容器域名

Python: SunMoonTimeCalculator

NETCore中實現一個輕量無負擔的極簡任務調度ScheduleTask

docker使用特定的網絡

使用c#強大的表達式樹實現對象的深克隆之解決循環引用的問題

「Pygors跨平臺GUI」2：安裝MinGW-w64、MSYS2還是WSL2

「Pygors跨平臺GUI」1：Pygors跨平臺GUI應用研究

nodejs學習07——API

避免DbContext同時在多個線程調用

GPT-4o 引領人機交互新風向，向量數據庫賽道沸騰了

SLAM前端：ICP（Iterative Closest Point）

卡爾曼濾波（KF）與擴展卡爾曼濾波（EKF）講解與推導

粒子濾波（PF）

最優化六：牛頓法（牛頓法、擬牛頓法、阻尼牛頓法）

最優化方法二：凸集、凸函數與凸優化

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結