对于无人机的惯性导航系统，系统的状态方程是非线性的，根据扩展卡尔曼滤波方程：
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 和hh。这里介绍两种形式的状态函数，第一种是不包含哥式校正（即不考虑地球自转以及无人机绕地球的速度所产生的向心加速度），一种是包含哥式校正的。这篇文章先介绍不包含哥式校正的EKF，包含哥式校正的将在下一篇文章介绍。
首先介绍各参数定义： L L :纬度，单位:deg∗107deg∗107 l l :经度，单位:deg∗107deg∗107 h h :高度，单位:mm 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 :时间间隔,单位:ss 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^)=[LlhvNvEvD] 根据雅可比矩阵定义，计算得状态和观测矩阵如下： 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 × 6Matlab仿真效果
