EKF 迎角估计算法

1.1 基于飞行动力学模型的状态方程建立

考虑到本文所涉及的问题具有一般性，因此可假设：

飞行器为理想刚体且质量为常数；

忽略地球曲率，重力加速度不随飞行高度变化；

飞行器为面对称布局飞行器，几何外形和内部质量分布均对称等。

飞行动力学模型是飞行器实现飞行和控制的基础，是预测飞行器的执行机构动作对飞行状态参数影响的数学工具。基于飞行动力学模型，考虑与攻角和侧滑角相关联的飞行状态参数，同时根据状态方程封闭性原则，选取飞行器的俯仰角、横滚角、横滚速率、俯仰角速率、航行角速率、攻角、侧滑角和飞行速度作为状态量，即 $\boldsymbol{X}=[\theta, \phi, p, q, r, \alpha, \beta, V]^T$，以机体坐标系为基准，进而建立状态方程

$$\dot{\boldsymbol{X}}(t) = f[\boldsymbol{X}(t), t] + \boldsymbol{G}(t)\boldsymbol{w}(t) \tag{1}$$

状态方程具体形式为：

$$

\begin{cases}

\dot{\theta} = q\cos\phi - r\sin\phi \\

\dot{\phi} = p + (r\cos\phi + q\sin\phi)\tan\theta \\

\dot{p} = c_1pr - c_2pq + c_3L + c_4N \\

\dot{q} = c_5pr - c_6(p^2 - r^2) + c_7M \\

\dot{r} = c_8pq - c_1qr + c_4L + c_9N \\

\dot{\alpha} = q - \frac{1}{mV\cos\beta}(L\cos\alpha + D\sin\alpha) + \frac{T_z}{mV\cos\beta} + \frac{g}{V\cos\beta}(\sin\alpha\sin\theta + \cos\alpha\cos\phi\cos\theta) \\

\dot{\beta} = \frac{1}{mV}(Y\cos\beta + L\sin\alpha\sin\beta - D\cos\alpha\sin\beta) + \frac{T_y}{mV} - p\cos\alpha + r\sin\alpha + \frac{g}{V}(\sin\beta\sin\phi\cos\theta + \sin\alpha\cos\beta\cos\phi\cos\theta - \cos\alpha\cos\beta\sin\theta) \\

\dot{V} = \frac{1}{m}(T_x - D\cos\alpha\cos\beta + Y\sin\beta + L\sin\alpha\cos\beta) - g(\sin\alpha\cos\beta\sin\theta - \sin\beta\sin\phi\cos\theta - \cos\alpha\cos\beta\cos\phi\cos\theta)

\end{cases} \tag{2}

$$

其中，系统噪声系数阵为：

$$

\boldsymbol{G}(t) =

\begin{bmatrix}

0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\

0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\

0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\

0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\

0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\

\frac{\cos\alpha}{mV\cos\beta} & \frac{-1}{mV\cos\beta} & 0 & 0 & 0 & \frac{1}{mV\cos\beta} & 0 & 0 \\

\frac{\sin\alpha\sin\beta}{mV} & \frac{-\cos\alpha\sin\beta}{mV} & \frac{\cos\beta}{mV} & 0 & 0 & 0 & \frac{1}{mV} & 0 \\

\frac{\sin\alpha\cos\beta}{m} & \frac{-\cos\alpha\cos\beta}{m} & \frac{\sin\beta}{m} & 0 & 0 & 0 & 0 & \frac{1}{m}

\end{bmatrix} \tag{3}

$$

惯量系数

$$

\begin{cases}

c_1 = \frac{(I_y - I_z)I_z - I_{xz}^2}{\Sigma} \\

c_2 = \frac{(I_x - I_y + I_z)I_{xz}}{\Sigma} \\

c_3 = \frac{I_z}{\Sigma} \\

c_4 = \frac{I_{xz}}{\Sigma} \\

c_5 = \frac{I_z - I_x}{I_y} \\

c_6 = \frac{I_{xz}}{I_y} \\

c_7 = \frac{1}{I_y} \\

c_8 = \frac{(I_x - I_y)I_x - I_{xz}^2}{\Sigma} \\

c_9 = \frac{I_x}{\Sigma} \\

\Sigma = I_xI_z - I_{xz}^2

\end{cases}

$$

式中：$L, M, N$ 和 $T_x, T_y, T_z$ 分别为合外力矩和发动机推力在机体轴系的3个轴向分量；$D, Y, L$ 是总空气动力沿气流坐标轴系各轴分量，分别表示阻力、侧力和升力，以上合外力矩、发动机推力以及空气动力可以根据飞行器结构特征参数、发动机模型、飞行状态量以及控制操纵输入获得；$c_1, c_2, \dots, c_9$ 为力矩方程系数，由飞行器的转动惯量和惯性积构成，根据以上假设条件，这9个系数为常数；另外，$m$ 为飞行器的质量；$g$ 是重力加速度。

状态方程中，系统噪声 $\boldsymbol{w}(t)=[\delta L, \delta M, \delta N, \delta T_x, \delta T_y, \delta T_z, \delta D, \delta Y, \delta L]^T$，分别由飞行器的合外力矩、空气动力以及发动机推力引起。

1.2 系统观测方程的建立

扩展卡尔曼滤波的观测方程直接反映观测系统的测量原理以及量测量与系统状态量之间的关系。机载惯导系统是测量飞控系统运动状态参数的主要信息来源，具有自主性强、测量精度高、机动敏感性强、数据丰富等特点，能够对高性能飞行器的大攻角高机动飞行状态进行精确跟踪，因此以惯导系统提供的俯仰角、横滚角、姿态角速率以及加速度信息作为量测信息，即 $\boldsymbol{Z}=[\theta, \phi, p, q, r, a_x, a_y, a_z]^T$，建立量测方程

$$\boldsymbol{z}(t) = h(\boldsymbol{X}(t), t) + \boldsymbol{v}(t) \tag{4}$$

量测方程具体可表示为：

$$

\begin{cases}

z_\theta = \theta \\

z_\phi = \phi \\

z_p = p \\

z_q = q \\

z_r = r \\

z_{a_x} = \frac{T_x + L\sin\alpha - D\cos\alpha}{m} \\

z_{a_y} = \frac{T_y + Y}{m} \\

z_{a_z} = \frac{T_z - L\cos\alpha - D\sin\alpha}{m}

\end{cases} + \boldsymbol{v}(t) \tag{5}

$$

式中：量测噪声 $\boldsymbol{v}(t)=[\delta\theta, \delta\phi, \delta p, \delta q, \delta r, \delta a_x, \delta a_y, \delta a_z]^T$，由惯性器件陀螺仪和加速度计的观测噪声引起。

1.3 状态方程和量测方程的线性化和离散化及扩展卡尔曼滤波器

由于状态方程和量测方程均是非线性的，首先用泰勒展开线性化再离散化，最终得到离散型线性干扰方程

$$

\begin{cases}

\delta\boldsymbol{X}{k+1} = \boldsymbol{\Phi}{k+1,k}\delta\boldsymbol{X}k + \boldsymbol{\Gamma}{k+1,k}\boldsymbol{w}_k \\

\delta\boldsymbol{Z}_k = \boldsymbol{H}_k\delta\boldsymbol{X}_k + \boldsymbol{v}_k

\end{cases} \tag{6}

$$

式中

$$\delta\boldsymbol{X} = \boldsymbol{X} - \hat{\boldsymbol{X}} \tag{7}$$

$$\delta\boldsymbol{Z} = \boldsymbol{Z} - \hat{\boldsymbol{Z}} \tag{8}$$

考虑到采样周期 $T$ 较小，系统转移阵 $\boldsymbol{\Phi}{k+1,k}$ 和系统噪声系数阵 $\boldsymbol{\Gamma}{k+1,k}$ 可分别近似为 $\boldsymbol{\Phi} \approx \boldsymbol{I} + \boldsymbol{F}(t)T$ 和 $\boldsymbol{\Gamma} \approx T(\boldsymbol{I} + (\boldsymbol{F}(t)/2)T)\boldsymbol{G}(t)$；另外，$\boldsymbol{F}(t)$、$\boldsymbol{H}(t)$ 分别为状态方程和量测方程的雅克比矩阵。

根据扩展卡尔曼滤波递推方程和所建立的攻角/侧滑角计算系统的状态方程和量测方程，得到系统滤波方程为：

$$

\begin{cases}

\hat{\boldsymbol{X}}{k+1/k} = \hat{\boldsymbol{X}}k + f(\hat{\boldsymbol{X}}k, k)T \\

\delta\hat{\boldsymbol{X}}{k+1} = \delta\hat{\boldsymbol{X}}{k+1/k} + \boldsymbol{K}{k+1}(\delta\boldsymbol{Z}{k+1} - \boldsymbol{H}{k+1}\delta\hat{\boldsymbol{X}}{k+1/k}) \\

\hat{\boldsymbol{X}}{k+1} = \hat{\boldsymbol{X}}{k+1/k} + \delta\hat{\boldsymbol{X}}{k+1} \\

\boldsymbol{K}{k+1} = \boldsymbol{P}{k+1/k}\boldsymbol{H}{k+1}^T[\boldsymbol{H}{k+1}\boldsymbol{P}{k+1/k}\boldsymbol{H}{k+1}^T + \boldsymbol{R}{k+1}]^{-1} \\

\boldsymbol{P}{k+1/k} = \boldsymbol{\Phi}{k+1,k}\boldsymbol{P}k\boldsymbol{\Phi}{k+1,k}^T + \boldsymbol{\Gamma}{k+1,k}\boldsymbol{Q}k\boldsymbol{\Gamma}{k+1,k}^T \\

\boldsymbol{P}{k+1} = (\boldsymbol{I} - \boldsymbol{K}{k+1}\boldsymbol{H}{k+1})\boldsymbol{P}{k+1/k}(\boldsymbol{I} - \boldsymbol{K}{k+1}\boldsymbol{H}{k+1})^T + \boldsymbol{K}{k+1}\boldsymbol{R}{k+1}\boldsymbol{K}_{k+1}^T

\end{cases} \tag{9}

$$

式中：$\boldsymbol{Q}$、$\boldsymbol{R}$ 分别为系统噪声 $\boldsymbol{W}$ 和量测噪声 $\boldsymbol{V}$ 的方差阵。