惯性导航——扩展卡尔曼滤波（一）

相關源碼請參考開源飛控StarryPilot:https://github.com/JcZou/StarryPilot

对于无人机的惯性导航系统，系统的状态方程是非线性的，根据扩展卡尔曼滤波方程：

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}

雅可比矩阵具体的含义可以参看Wiki:雅可比矩阵

首先需要确定 $f$ 和 $h$ 。这里介绍两种形式的状态函数，第一种是不包含哥式校正（即不考虑地球自转以及无人机绕地球的速度所产生的向心加速度），一种是包含哥式校正的。这篇文章先介绍不包含哥式校正的EKF，包含哥式校正的将在下一篇文章介绍。

首先介绍各参数定义：
$L$ :纬度，单位: $d e g * 10^{7}$
$l$ :经度，单位: $d e g * 10^{7}$
$h$ :高度，单位: $m$
$v_{N}$ :朝北速度，单位: $m / s$
$v_{E}$ :朝东速度，单位: $m / s$
$v_{D}$ :朝地速度，单位: $m / s$
$R_{0}$ :地球平均半径
$T_{x}$ :x转换算子，用于将南北向位移量转化为纬度变化量， $T_{x} = \frac{180 * 10^{7}}{π R_{0}}$ (假设在近地面飞行，忽略高度对半径的影响。如要考虑，则分母变为 $π (R_{0} + h)$ )
$T_{y}$ :y转换算子，用于将朝东西向位移量转化为经度变化量， $T_{y} = \frac{180 * 10^{7}}{π R_{0} \sin (90 - L * 10^{- 7})} = \frac{180 * 10^{7} * \sec (L * 10^{- 7})}{π R_{0}}$ (同样假设在近地面飞行)
$T$ :时间间隔,单位: $s$
$a_{N}$ :朝北加速度,单位: $m / t^{2}$
$a_{E}$ :朝东加速度,单位: $m / t^{2}$
$a_{D}$ :朝地加速度，已做gravity corectify,即有加上重力常量g,单位: $m / t^{2}$

f (\hat{x}, u) = [\begin{matrix} L + v_{N} T_{x} T \\ l + v_{E} T_{y} T \\ h - v_{D} T \\ v_{N} + a_{N} T \\ v_{E} + a_{E} T \\ v_{D} + a_{D} T \end{matrix}]

其中，

\hat{x} = [\begin{matrix} L & l & h & v_{N} & v_{E} & v_{D} \end{matrix}]

，

u = [\begin{matrix} a_{N} & a_{E} & a_{D} \end{matrix}]

.
由于状态的观测量可以由传感器直接获取到，所以

h

的定义如下：

h (\hat{x}) = [\begin{matrix} L \\ l \\ h \\ v_{N} \\ v_{E} \\ v_{D} \end{matrix}]

根据雅可比矩阵定义，计算得状态和观测矩阵如下：

F_{k - 1} = [\begin{matrix} 1 & 0 & 0 & T_{x} T & 0 & 0 \\ \frac{180 T v_{E} \sin (L * 10^{- 7})}{π R_{0} \cos (L * 10^{- 7})^{2}} & 1 & 0 & 0 & \frac{180 * 10^{7} T}{π R_{0} \cos (L * 10^{- 7})} & 0 \\ 0 & 0 & 1 & 0 & 0 & - T \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}]

H_{k} = I_{6 \times 6}

Matlab仿真效果
惯性导航——扩展卡尔曼滤波（一）

惯性导航——扩展卡尔曼滤波（一）

相關源碼請參考開源飛控StarryPilot:https://github.com/JcZou/StarryPilot

相关推荐