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

大大源码 • 2023年3月10日 pm11:23 • 其他

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

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

Predict

x^k | k - 1 P k | k - 1 = f (x^k - 1 | k - 1, u k - 1) = F k - 1 P k - 1 | k - 1 F T k - 1 + Q k - 1 (43)

$\begin{equation} \begin{split} \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{split} \end{equation}$

Update

y ~ k S k K k x^k | k P k | k = z k - h (x^k | k - 1) = H k P k | k - 1 H T k + R k = P k | k - 1 H T k S - 1 k = x^k | k - 1 + K k y ~ k = (I - K k H k) P k | k - 1 (44)

$\begin{equation} \begin{split} \tilde{y}_k&=z_k-h(\hat{x}_{k|k-1}) \\ S_k&=H_kP_{k|k-1}H_k^T+R_k \\ K_k&=P_{k|k-1}H_k^TS_k^{-1} \\ \hat{x}_{k|k}&=\hat{x}_{k|k-1}+K_k\tilde{y}_k \\ P_{k|k}&=(I-K_kH_k)P_{k|k-1} \\ \end{split} \end{equation}$

其中状态和观测矩阵为状态和观测函数的雅可比矩阵：

F k - 1 H k = \partial f \partial x | x^k - 1 | k - 1, u k - 1 = \partial h \partial x | x^k | k - 1 (45)

$\begin{equation} \begin{split} 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{split} \end{equation}$

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

首先需要确定
f

$f$ 和 $h$

h

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

首先介绍各参数定义：

L

$L$ :纬度，单位: $d e g * 10^{7}$

d e g * 10^{7}

$deg*10^7$

l

$l$ :经度，单位: $d e g * 10^{7}$

d e g * 10^{7}

$deg*10^7$

h

$h$ :高度，单位: $m$

m

$m$

vN

$v_N$ :朝北速度，单位:
m/s

$m/s$

vE

$v_E$ :朝东速度，单位:
m/s

$m/s$

vD

$v_D$ :朝地速度，单位:
m/s

$m/s$

R0

$R_0$ :地球平均半径

Tx

$T_x$ :x转换算子，用于将南北向位移量转化为纬度变化量，
Tx=180∗107πR0

180

∗

$T_x=\frac{180*10^7}{\pi R_0}$ (假设在近地面飞行，忽略高度对半径的影响。如要考虑，则分母变为
π(R0+h)

(

)

$\pi(R_0+h)$ )

Ty

$T_y$ :y转换算子，用于将朝东西向位移量转化为经度变化量，
Ty=180∗107πR0sin(90−L∗10−7)=180∗107∗sec(L∗10−7)πR0

180

∗

sin

⁡

(

−

∗

−

)

180

∗

sec

⁡

(

∗

−

)

$T_y=\frac{180*10^7}{\pi R_0\sin(90-L*10^{-7})}=\frac{180*10^7*\sec(L*10^{-7})}{\pi R_0}$ (同样假设在近地面飞行)

T

$T$ :时间间隔,单位: $s$

s

$s$

aN

$a_N$ :朝北加速度,单位:
m/t2

$m/t^2$

aE

$a_E$ :朝东加速度,单位:
m/t2

$m/t^2$

aD

$a_D$ :朝地加速度，已做gravity corectify,即有加上重力常量g,单位:
m/t2

$m/t^2$

f (x^, u) = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 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 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$f(\hat{x},u)=\begin{bmatrix} L+v_NT_xT\\ l+v_ET_yT\\ h-v_DT\\ v_N+a_NT\\ v_E+a_ET\\ v_D+a_DT \end{bmatrix}$

其中，

x^=[LlhvNvEvD]

[

]

$\hat{x}=\begin{bmatrix}L&l&h&v_N&v_E&v_D\end{bmatrix}$ ，

u=[aNaEaD]

[

]

$u=\begin{bmatrix}a_N&a_E&a_D\end{bmatrix}$ .

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

h

$h$ 的定义如下：

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

$h(\hat{x})=\begin{bmatrix} L\\ l\\ h\\ v_N\\ v_E\\ v_D \end{bmatrix}$

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

F k - 1 = ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ 1 180 T v E sin ( L * 10 - 7 ) π R 0 cos ( L * 10 - 7 ) 2 0000 010000001000 T x T 00100 0 180 * 10 7 T π R 0 cos ( L * 10 - 7 ) 0010 00 - T 001 ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥

$F_{k-1}=\begin{bmatrix} 1 & 0 & 0 & T_xT & 0 & 0 \\ \frac{180Tv_E\sin(L*10^{-7})}{\pi R_0\cos(L*10^{-7})^2} & 1 & 0 & 0 & \frac{180*10^7T}{\pi 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{bmatrix}$