飞行器动力学参数在线辨识EKF算法

实验流程

  飞行器俯仰通道动力学微分方程是连续非线性方程，要进行扩展卡尔曼滤波（EKF）的步骤是：先进行线性化，再进行离散化。

飞行器俯仰通道动力学方程线性化

参考文献：永磁同步电机的无速度传感器控制算法研究 冯晓彤[D]
 
  针对一般的连续非线性系统，有
$$\dot{x}=f(x,t)+B(t)u(t)\text{\hspace{1em}(1)}$$$$z=h(x,t)\text{\hspace{1em}(2)}$$
在前一时刻的滤波估计值${\hat{x}}_k$下用泰勒展开线性化，得到
$$\dot{x}(t)=f(x,t){|}_{x={\hat{x}}_k}+\frac{\partial f(x,t)}{\partial x}{|}_{x={\hat{x}}_k}\Delta x(t)+B(t)u(t)+G(t)w(t)\text{\hspace{1em}(3)}$$$$z(t)=h(x,t){|}_{x={\hat{x}}_k}+\frac{\partial h(x,t)}{\partial x}{|}_{x={\hat{x}}_k}\Delta x(t)+v(t)\text{\hspace{1em}(4)}$$$$\Delta x(t)=x(t)+{\hat{x}}_k\text{\hspace{1em}(5)}$$

经过适当的变换，可以得到线性连续方程
$$\Delta \dot{x}(t)=F(t)\Delta x+Gw(t)\text{\hspace{1em}(6)}$$$$\Delta z(t)=H(t)\Delta x+v(t)\text{\hspace{1em}(7)}$$
其中
F ( t ) = [