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

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

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

---

Predict

x^k|k−1Pk|k−1=f(x^k−1|k−1,uk−1)=Fk−1Pk−1|k−1FTk−1+Qk−1(43) (43) x ^ k | k − 1 = f ( 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

Update

y~kSkKkx^k|kPk|k=zk−h(x^k|k−1)=HkPk|k−1HTk+Rk=Pk|k−1HTkS−1k=x^k|k−1+Kky~k=(I−KkHk)Pk|k−1(44) (44) y ~ k = z k − h ( 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 x ^ k | k = x ^ k | k − 1 + K k y ~ k P k | k = ( I − K k H k ) P k | k − 1

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

Fk−1Hk=∂f∂x|x^k−1|k−1,uk−1=∂h∂x|x^k|k−1(45) (45) F k − 1 = ∂ f ∂ x | x ^ k − 1 | k − 1 , u k − 1 H k = ∂ h ∂ x | x ^ k | k − 1

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

---

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

首先介绍各参数定义：
L L :纬度，单位: deg∗107 d e g ∗ 10 7
l l :经度，单位: deg∗107 d e g ∗ 10 7
h h :高度，单位: 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 T x = 180 ∗ 10 7 π R 0 (假设在近地面飞行，忽略高度对半径的影响。如要考虑，则分母变为 π(R0+h) π ( R 0 + h ) )
Ty T y :y转换算子，用于将朝东西向位移量转化为经度变化量， Ty=180∗107πR0sin(90−L∗10−7)=180∗107∗sec(L∗10−7)πR0 T y = 180 ∗ 10 7 π R 0 sin ⁡ ( 90 − L ∗ 10 − 7 ) = 180 ∗ 10 7 ∗ sec ⁡ ( L ∗ 10 − 7 ) π R 0 (同样假设在近地面飞行)
T T :时间间隔,单位: 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+vNTxTl+vETyTh−vDTvN+aNTvE+aETvD+aDT⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥ 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 ]

其中， x^=[LlhvNvEvD] x ^ = [ L l h v N v E v D ] ， u=[aNaEaD] u = [ a N a E a D ] .
由于状态的观测量可以由传感器直接获取到，所以 h h 的定义如下：

h(x^)=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢LlhvNvEvD⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥ h ( x ^ ) = [ L l h v N v E v D ]

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

Fk−1=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢1180TvEsin(L∗10−7)πR0cos(L∗10−7)20000010000001000TxT001000180∗107TπR0cos(L∗10−7)001000−T001⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥ F k − 1 = [ 1 0 0 T x T 0 0 180 T v E sin ⁡ ( L ∗ 10 − 7 ) π R 0 cos ⁡ ( L ∗ 10 − 7 ) 2 1 0 0 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 ]

Hk=I6×6 H k = I 6 × 6

---

Matlab仿真效果