慣性導航——擴充套件卡爾曼濾波（一）

對於無人機的慣性導航系統，系統的狀態方程是非線性的，根據擴充套件卡爾曼濾波方程：

Predict

\begin{matrix} (43) & \begin{aligned} {\hat{x}}_{k | k - 1} & = f ({\hat{x}}_{k - 1 | k - 1}, u_{k - 1}) \\ P_{k | k - 1} & = F_{k - 1} P_{k - 1 | k - 1} F_{k - 1}^{T} + Q_{k - 1} \end{aligned} \end{matrix}

Update

\begin{matrix} (44) & \begin{aligned} {\tilde{y}}_{k} & = z_{k} - h ({\hat{x}}_{k | k - 1}) \\ S_{k} & = H_{k} P_{k | k - 1} H_{k}^{T} + R_{k} \\ K_{k} & = P_{k | k - 1} H_{k}^{T} S_{k}^{- 1} \\ {\hat{x}}_{k | k} & = {\hat{x}}_{k | k - 1} + K_{k} {\tilde{y}}_{k} \\ P_{k | k} & = (I - K_{k} H_{k}) P_{k | k - 1} \end{aligned} \end{matrix}

其中狀態和觀測矩陣為狀態和觀測函式的雅可比矩陣：

\begin{matrix} (45) & \begin{aligned} F_{k - 1} & = \frac{\partial f}{\partial x} |_{{\hat{x}}_{k - 1 | k - 1}, u_{k - 1}} \\ H_{k} & = \frac{\partial h}{\partial x} |_{{\hat{x}}_{k | k - 1}} \end{aligned} \end{matrix}