惯性导航——扩展卡尔曼滤波(一)

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

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


Predict

x^k|k1Pk|k1=f(x^k1|k1,uk1)=Fk1Pk1|k1FTk1+Qk1(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=zkh(x^k|k1)=HkPk|k1HTk+Rk=Pk|k1HTkS1k=x^k|k1+Kky~k=(IKkHk)Pk|k1(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

其中状态和观测矩阵为状态和观测函数的雅可比矩阵:
Fk1Hk=fx|x^k1|k1,uk1=hx|x^k|k1(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 :纬度,单位: deg107 d e g ∗ 10 7
l l :经度,单位: deg107 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=180107πR0 T x = 180 ∗ 10 7 π R 0 (假设在近地面飞行,忽略高度对半径的影响。如要考虑,则分母变为 π(R0+h) π ( R 0 + h ) )
Ty T y :y转换算子,用于将朝东西向位移量转化为经度变化量, Ty=180107πR0sin(90L107)=180107sec(L107)π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+vETyThvDTvN+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 ]

根据雅可比矩阵定义,计算得状态和观测矩阵如下:
Fk1=1180TvEsin(L107)πR0cos(L107)20000010000001000TxT001000180107TπR0cos(L107)001000T001 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仿真效果
惯性导航——扩展卡尔曼滤波(一)_第1张图片
惯性导航——扩展卡尔曼滤波(一)_第2张图片

你可能感兴趣的:(无人机)