CN103363993A - 一种基于无迹卡尔曼滤波的飞机角速率信号重构方法 - Google Patents

Abstract

本发明公开了一种基于无迹卡尔曼滤波的飞机角速率信号重构方法，包括以下步骤：1)将含有测量噪声的姿态角测量信号通过非线性的跟踪微分器进行消噪处理，得到姿态角的微分信号，构建虚拟的量测方程；2)考虑飞机的自然特性，根据飞机三轴的角速率信号和力矩特性构建系统的状态方程；3)因状态方程和量测方程均为非线性的，传统的将非线性方程线性化的操作会使得估计精度受到影响，因此选用无迹卡尔曼滤波技术作为处理手段，实现系统的角速率信号重构。

Description

一种基于无迹卡尔曼滤波的飞机角速率信号重构方法

技术领域

本发明属于航空技术领域，涉及一种基于无迹卡尔曼滤波的飞机角速率信号重构方法。

背景技术

自成功突破音障后，飞机的飞行包线逐渐扩大，飞机自身的稳定性逐步恶化；且随着飞行高度增加，飞机自身的阻尼力矩也逐渐变小，使得飞机操纵困难。飞机的角速率反馈是增加系统阻尼的关键反馈信号，直接影响到飞机的稳定性。因此，对角速率传感器的故障诊断、信号重构的研究就显得愈加重要。现代飞机的传感器一般采用余度技术来提高可靠性，所谓余度技术就是引入多重系统执行同一工作。但是余度技术的引入也带来一定的问题，如使系统的复杂性增加，提高了设计和研制的成本。此外，它还将可能成为飞行控制系统额外的故障源。鉴于硬件余度的不足，可通过其他方法重构传感器信号来代替硬件传感器信号。采用解析冗余信息可重构角速率信号，当所有传感器都故障时，可直接利用重构信号作为反馈实现闭环控制。

现有技术中利用降维观测器对俯仰角速率信号进行了重构研究，但其研究对象是线性的飞机模型，因此不能直接应用到非线性飞机。有的利用扩展反向传播神经网络方法重构俯仰角速率信号，需要使用到当前的迎角、升降舵偏角、俯仰角、法向过载及这些信号前4个周期的历史信息，该方法操作复杂，不适于工程应用。有的技术给出了两种俯仰角速率重构方法：其一是建立等效的线性模型，设计线性状态观测器重构状态信号；其二是使用非线性跟踪微分器方法，通过跟踪俯仰角的微分信号来计算俯仰角速率。方法一中迎角的观测误差对重构信号的精度有较大的影响，而方法二中指出基于非线性跟踪微分器和姿态角微分与角速率解析关系的方法重构俯仰角速率信号仅适用于滚转角小于70°的情况。

发明内容

本发明的目的在于克服上述技术存在的缺陷，提供一种基于无迹卡尔曼滤波的飞机角速率信号重构方法，将非线性跟踪微分器输出的姿态角的微分信号作为无迹卡尔曼滤波器的观测量，给出了在角速率传感器故障情况下的重构信号。所得估计信号能够以足够的精度接近真实信号，且在大机动飞行情况下仍具有比较好的估计效果。所提出的算法具有估计精度高，实时性好，且具有一定的鲁棒性，因此该方法具有一定的可行性。并且本发明不受特定飞机模型的限制，可直接用于各种飞机的飞行控制系统角速率信号的重构。

为实现上述目的，本发明采用以下技术方案：

一种基于无迹卡尔曼滤波的飞机角速率信号重构方法，利用姿态角的微分信号作为滤波器的观测量，并且微分信号是利用非线性的跟踪微分器获取的，这避免了利用常规的差分方法使得噪声被严重放大的缺陷；同时利用的是飞机机载设备的输出参数，包括：

(1)捷联惯性导航系统输出的姿态角；

(2)飞行控制系统输出的合外力矩；

(3)非线性跟踪微分器输出的姿态角的微分信号；

由飞机的转动惯量和惯性积计算得到力矩方程组的系数c₁，c₂L，c₉；

由捷联惯导系统输出的姿态角信号经非线性跟踪微分器得到姿态角的微分；

根据飞行动力学模型，构建无迹卡尔曼滤波器，状态向量选取角速率信号，观测向量选取姿态角的微分信号；利用无迹卡尔曼滤波算法，实现当前时刻飞机三轴角速率信号的精确估计。

将当前时刻的角速率信号的精确估计值反馈给无迹卡尔曼滤波算法模块，用于完成下一时刻的角速率信号的实时精确估计，通过递推求解的方式实现角速率信号的实时精确估计。

包括以下步骤：

(1)以周期T读取捷联惯导系统中的三个姿态角信息，三个姿态角分别为俯仰角θ、滚转角φ、偏航角ψ；

(2)以周期T读取飞行控制系统输出的飞机所受到的合外力矩，合外力矩在机体坐标系下x轴、y轴和z轴的分量分别为滚转力矩

俯仰力矩M和偏航力矩N；

(3)利用飞机的转动惯量(I_x，I_y，I_z)和惯性积I_xz按照下式计算力矩方程组的系数

$c_1=\frac{(I_y-I_z)I_z-{I_{\mathrm{xz}}}^2}{\mathrm{\Σ}},$ $c_2=\frac{(I_x-I_y+I_z)I_{\mathrm{xz}}}{\mathrm{\Σ}},$ $c_3=\frac{I_z}{\mathrm{\Σ}},$ $c_4=\frac{I_{\mathrm{xz}}}{\mathrm{\Σ}},$ $c_5=\frac{I_z-I_x}{I_y},$ $c_6=\frac{I_{\mathrm{xz}}}{I_y},$ $c_7=\frac{1}{I_y},$ $c_8=\frac{(I_x-I_y)I_x-{I_{\mathrm{xz}}}^2}{\mathrm{\Σ}},$ $c_9=\frac{I_x}{\mathrm{\Σ}},$ ∑＝I_xI_z-I_xz ²；

(4)将步骤(1)获取的飞机的俯仰角θ、滚转角φ、偏航角ψ分别输入到非线性跟踪微分器得到俯仰角微分

滚转角微分

和偏航角的微分

$\left\{\begin{array}{c}{x}_1(k+1)=x_1\left(k\right)+h*x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)+h*\mathrm{fst}2(x_1\left(k\right),x_2\left(k\right),\mathrm{vv}\left(k\right),r,h_1)\end{array}\right.$

上式中将含测量噪声的θ、φ和ψ分别代替vv(k)，则相应的x₂(k)分别对应于

和

其中h为离散型非线性跟踪微分器的仿真步长，这里取h＝T，r和h₁分别为滤波因子和快慢因子。

(5)根据飞行动力学模型，选取飞机的俯仰角速率q、滚转角速率p和偏航角速率r作为状态变量，即状态向量X＝[p q r]^T，进而建立无迹卡尔曼滤波器的状态方程；选取步骤(4)得到的俯仰角微分

滚转角微分

和偏航角微分

作为观测变量，即观测向量为

进而建立无迹卡尔曼滤波器量测方程。

(6)根据步骤(1)和步骤(4)获取的当前时刻即t_k+1时刻的量测信息，步骤(2)获取的上一时刻即t_k时刻的合外力矩，步骤(3)计算得到的9个力矩方程组系数，利用无迹卡尔曼滤波方程得到t_k+1时刻状态量的最优估计值，从而实现t_k+1时刻的三轴角速率信号的实时精确估计。

(7)将步骤(6)得到的t_k+1时刻的角速率信号的精确估计值反馈给无迹卡尔曼滤波算法模块，用于完成步骤(6)中的下一时刻即t_k+2时刻角速率信号的估计。

进一步优选，所述步骤(6)中无迹卡尔曼滤波算法的具体步骤是：

(a)无迹卡尔曼滤波器状态方程的建立

根据飞行动力学模型，选取飞机的俯仰角速率q、滚转角速率p和偏航角速率r作为状态变量，即状态向量X＝[p q r]^T，进而建立无迹卡尔曼滤波器的状态方程：

状态方程的具体形式为：

其中，u(t)为控制输入量，包括升降舵偏角δe，副翼偏转角δa和方向舵偏角δr，滚转力矩

俯仰力矩M以及偏航力矩由下式计算得到

$\stackrel{\‾}{L}=f_{\stackrel{\‾}{L}}(\mathrm{\δe},\mathrm{\δa},\mathrm{\δr},\mathrm{\α},\mathrm{\β},p,q,r,L)$

M＝f_M(δe，δa，δr，α，β，p，q，r，L)

N＝f_N(δe，δa，δr，α，β，p，q，r，L)

w(t)为激励噪声序列，这里取为高斯白噪声。

(b)无迹卡尔曼滤波器量测方程的建立

将由捷联惯导系统获取的俯仰角θ、滚转角φ、偏航角ψ分别输入到非线性跟踪微分器得到俯仰角微分滚转角微分

和偏航角的微分

其中非线性跟踪微分器的离散形式为：

$\left\{\begin{array}{c}{x}_1(k+1)=x_1\left(k\right)+h*x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)+h*\mathrm{fst}2(x_1\left(k\right),x_2\left(k\right),\mathrm{vv}\left(k\right),r,h_1)\end{array}\right.$

vv(k)为含噪声的输入信号，x₁(k)用于跟踪输入信号vv(k)，而x₂(k)则跟踪vv(k)的微分信号。在上式中将含测量噪声的θ、φ和ψ分别代替vv(k)，则相应的x₂(k)分别对应于

和

其中h为离散型非线性跟踪微分器的仿真步长，这里取h＝T，r和h₁分别为滤波因子和快慢因子。

选取俯仰角微分

滚转角微分和偏航角微分

作为观测变量，即观测向量为进而建立无迹卡尔曼滤波器量测方程：

Z(t)＝h[X(t)，u(t)，t]+v(t)

量测方程具体可表示为：

v(t)为量测噪声序列，同样选择为高斯白噪声。

(c)状态方程和观测方程的离散化

假定离散化时间为T，对状态方程离散化有

X(k+1)＝F(X(k)，u(k)，T)·X(k)+w(k)

即：

$\left[\begin{array}{c}p(k+1)\\ q(k+1)\\ r(k+1)\end{array}\right]=\left[\begin{array}{ccc}1+f_1(\·)\·T& 0& 0\\ 0& 1+f_2(\·)\·T& 0\\ 0& 0& 1+f_3(\·)\·T\end{array}\right]\·\left[\begin{array}{c}p\left(k\right)\\ q\left(k\right)\\ r\left(k\right)\end{array}\right]+\left[\begin{array}{c}{w}_1\left(k\right)\\ w_2\left(k\right)\\ w_3\left(k\right)\end{array}\right]$

对观测方程进行离散化有

Z(k+1)＝h(X(k+1))+v(k)

即：

其中φ(k+1)、θ(k+1)、ψ(k+1)分别为(k+1)时刻的滚转角、俯仰角和偏航角测量值，由捷联惯导系统输出。w(k)＝[w₁(k) w₂(k) w₃(k)]^T，v(k)＝[v₁(k) v₂(k) v₃(k)]^T分别为系统激励噪声序列和观测噪声序列，满足：

E[w(k)]＝0，Cov[w(k)，w(j)]＝E[w(k)w^T(j)]＝Q_kδ_kj

E[v(k)]＝0，Cov[v(k)，v(j)]＝E[v(k)v^T(j)]＝R_kδ_kj

Cov[w(k)，v(j)]＝E[w(k)v^T(j)]＝0

式中， ${\left(Q_k\right)}_{3\×3}=\left[\begin{array}{ccc}{\mathrm{\σ}}_{w_1\left(k\right)}^2& 0& 0\\ 0& {\mathrm{\σ}}_{w_2\left(k\right)}^2& 0\\ 0& 0& {\mathrm{\σ}}_{w_3\left(k\right)}^2\end{array}\right],$ ${\left(R_k\right)}_{3\×3}=\left[\begin{array}{ccc}{\mathrm{\σ}}_{v_1\left(k\right)}^2& 0& 0\\ 0& {\mathrm{\σ}}_{v_2\left(k\right)}^2& 0\\ 0& 0& {\mathrm{\σ}}_{v_3\left(k\right)}^2\end{array}\right]$ 分别为激励噪声矢量的协方差矩阵和观测噪声矢量的协方差矩阵，Q_k为非负定矩阵，R_k为正定矩阵。

为相关噪声矢量的标准方差常数。δ为狄拉克函数，满足

$\left\{\begin{array}{cc}\mathrm{\&delta;}(x-c)=0& x\&NotEqual;c\\ {\&Integral;}_a^b\mathrm{\&delta;}(x-c)=1& a<c<b\end{array}\right.$

(d)无迹卡尔曼滤波算法

假定三轴角速率的初始估计值 ${\hat{X}}_0={\left[\begin{array}{ccc}{p}_0& q_0& r_0\end{array}\right]}^T,$ 初始条件下的估计均方误差阵为 $P_{k0}=\mathrm{diag}\left[\begin{array}{ccc}{\mathrm{\&sigma;}}_{p0}^2& {\mathrm{\&sigma;}}_{q0}^2& {\mathrm{\&sigma;}}_{r0}^2\end{array}\right],$ 分别是滚转角速率，俯仰角速率，偏航角速率的初始估计方差。

选择对称采样策略，则相应的均值加权值W_k ^(m)和方差加权值W_k ^(c)分别可表示为：

${W_0}^{\left(m\right)}=\frac{\mathrm{\&lambda;}}{n+\mathrm{\&lambda;}}$

${W_0}^{\left(c\right)}=\frac{\mathrm{\&lambda;}}{n+\mathrm{\&lambda;}}+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})$

${W_i}^{\left(m\right)}={W_i}^{\left(c\right)}=\frac{1}{2\&CenterDot;(n+\mathrm{\&lambda;})},i=1,L2n$

其中n为状态维数，本文中n＝3；λ＝α²(n+κ)-n为尺度因子，κ用于保证方差矩阵的半正定性，一般取κ＝0或κ＝3-n，其取值大小对算法影响并不大；α为比例修正参数(常取1e-4≤α＜1)，以避免在系统非线性较强时的非局部采样。β用于引入状态先验分布的高阶项信息，取值范围β≥0，对于高斯分布，β＝2最优；对于非高斯分布，该参数还具有控制误差的作用，可以控制后验分布拖尾的大小。

系统状态Sigma点采样

根据k-1时刻的系统状态估计值

和协方差矩阵P_k-1|k-1进行Sigma点采样。对称采样Sigma点样本个数为L＝2n+1＝7个，则对应于k-1时刻的Sigma采样点为

${\mathrm{\&chi;}}_{k-1}=\left[\begin{array}{ccc}{\hat{X}}_{k-1|k-1}& {\hat{X}}_{k-1|k-1}-\sqrt{(L+\mathrm{\&lambda;})P_{k-1|k-1}}& {\hat{X}}_{k-1|k-1}+\sqrt{(L+\mathrm{\&lambda;})P_{k-1|k-1}}\end{array}\right]$

UKF时间更新

根据离散化的状态方程式，对上述采样的7个Sigma点进行状态预测，则有

χ_i，k，k-1＝F(χ_i，k-1)   i＝0，1L，6

利用上述采样预测值确定系统状态向量和协方差矩阵的最终预测值为：

${\hat{X}}_{k|k-1}=\underset{i=0}{\overset{6}{\mathrm{\&Sigma;}}}{W_i}^m\&CenterDot;{\mathrm{\&chi;}}_{i,k|k-1}$

$P_{k|k-1}=\underset{i=0}{\overset{6}{\mathrm{\&Sigma;}}}{W_i}^c({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1}){({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1})}^T+Q_k$

η_k|k-1＝h(χ_k|k-1)

${\hat{Z}}_{k|k-1}=\underset{i=0}{\overset{6}{\mathrm{\&Sigma;}}}{W_i}^m\&CenterDot;{\mathrm{\&eta;}}_{i,k|k-1}$

UKF量测更新

$P_{\mathrm{ZZk}|k-1}=\underset{i=0}{\overset{6}{\mathrm{\&Sigma;}}}{{W_i}^{\left(c\right)}({\mathrm{\&eta;}}_{i,k|k-1}-{\hat{Z}}_{k|k-1})\&CenterDot;({\mathrm{\&eta;}}_{i,k|k-1}-{\hat{Z}}_{k|k-1})}^T+R_k$

$P_{\mathrm{XZk}|k-1}=\underset{i=0}{\overset{6}{\mathrm{\&Sigma;}}}{W_i}^{\left(c\right)}({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1})\&CenterDot;{({\mathrm{\&eta;}}_{i,k|k-1}-{\hat{Z}}_{k|k-1})}^T$

$K_k=P_{\mathrm{XZk}|k-1}\&CenterDot;{\left(P_{\mathrm{ZZk}|k-1}\right)}^{-1}$

${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k(Z_k-{\hat{Z}}_{k|k-1})$

$P_k=P_{k|k-1}-K_k{P}_{\mathrm{ZZk}|k-1}{K}_k^T$

其中的为k时刻的系统观测值，由非线性跟踪微分器输出获得。

上述算法过程可以进一步概括为：首先根据系统状态的统计特性

和P_k0，选择一种采样策略得到相应的Sigma点集，现今广泛使用的Sigma点采样策略主要包括对称采样、最小偏度单形采样、超球面单形采样、比例修正采样、高斯分布4阶矩对称采样以及3阶矩偏度采样等，本文选择比例对称采样策略得到Sigma采样点集；将采样得到的Sigma点集通过非线性的状态方程进行传播，得到变换后的Sigma点集；对变换后的Sigma点集进行相应的加权处理，得到状态的一步预测值

以及一步预测均方误差矩阵P_k|k-1。然后将经过非线性变换后的Sigma点集通过非线性的量测方程进行传播并加权处理，得到观测量的一步预测值

协方差矩阵P_ZZ，k|k-1以及滤波增益矩阵K_k。利用测量值Z_k与观测量的一步预测值以及滤波增益K_k去修正状态的一步预测

从而得到状态的估计值

滤波过程得以完成，最终可以得到飞机三轴角速率的重构信号。

本发明的优点及其显著效果：本发明在不改变飞机的结构外形、不增添额外测量装置和硬件设备的前提下，充分利用机载惯性导航系统及飞行控制系统的输出参数，基于飞行动力学模型，通过构建无迹卡尔曼滤波器实现飞机三轴角速率信号的精确估计；本发明基于飞行动力学模型，不受限于具体的飞机型号，因此可以适用于任何飞机的角速率重构；与传统的仅仅利用非线性跟踪微分器和姿态角速率

与机体坐标系的三个角速率分量(p，q，r)的解析关系的方法相比，本发明能够给出更为准确的结果，并且可以拓宽系统的应用范围。

本发明的有益效果：与传统的仅仅利用非线性跟踪微分器和姿态角速率

与机体坐标系的三个角速率分量(p，q，r)的解析关系的方法相比，本发明能够给出更为准确的结果，并且可以拓宽系统的应用范围。

附图说明

图1为本发明基于无迹卡尔曼滤波的飞机角速率信号重构方法的流程示意图。

图2为非线性跟踪微分器与差分方法计算微分信号的结果对比图。

图3为非线性跟踪微分器输入输出示意图。

图4为重构的滚转角速率与真实的滚转角速率对比曲线。

图5为重构的滚转角速率与真实的滚转角速率的误差曲线。

图6为重构的俯仰角速率与真实的俯仰角速率对比曲线。

图7为重构的俯仰角速率与真实的俯仰角速率的误差曲线。

图8为重构的偏航角速率与真实的偏航角速率对比曲线。

图9为重构的偏航角速率与真实的偏航角速率的误差曲线。

具体实施方式

下面结合附图和具体实例对本发明的技术方案作进一步详细地说明。

参照图1，一种基于无迹卡尔曼滤波的飞机角速率信号重构方法，包括以下步骤：

1)状态方程的建立

考虑问题的一般性，假定以下条件：

(1)飞机为刚体，认为其质量为常数；

(2)将地球视为惯性系统，忽略地球的自转和公转的影响；

(3)忽略地球曲率，即采用所谓的“平板地球假设”；

(4)假设飞机为面对称布局，即惯性积I_xz和I_zy等于零。

刚体飞机的空间运动可用三个线坐标和三个角坐标的六自由度关系来描述，即飞机质心的位移(线运动——包括飞行速度的增减运动以及升降运动和侧移运动)，以及绕质心的转动(角运动——包括俯仰角运动和偏航角运动以及滚转角运动)。飞机在外力作用下的运动规律一般是用运动方程来描述的，即应用微分方程的形式来描述飞机的运动和状态参数随时间的变化规律。飞机的运动方程是由12个状态变量描述的一组封闭的微分方程。只要已知飞机相关的特征参数，根据飞行高度h、马赫数Ma以及飞行状态就可以确定力和力矩，应用12个微分方程就可以求解飞机在任何时刻的运动状态。但是由于复杂的非线性关系，运动状态的获得也不是那么容易。

在飞机运动方程中与角速率有关的方程包括力矩方程组和运动方程组。因此状态方程和量测方程的选取主要是围绕力矩方程和运动方程来的，选择状态变量X＝[p q r]^T，则状态方程为：

状态方程的具体形式为：

其中c₁，c₂，L，c₉为力矩方程系数，由飞机的转动惯量和惯性积获得，且为常数；u(t)为控制输入量，包括升降舵偏角δe，副翼偏转角δa和方向舵偏角δr，滚转力矩

、俯仰力矩M以及偏航力矩由下式计算得到，其中α，β分别为迎角和侧滑角；w(t)为激励噪声序列，这里取为高斯白噪声。

$\stackrel{\&OverBar;}{L}=f_{\stackrel{\&OverBar;}{L}}(\mathrm{\&delta;e},\mathrm{\&delta;a},\mathrm{\&delta;r},\mathrm{\&alpha;},\mathrm{\&beta;},p,q,r,L)$

M＝f_M(δe，δa，δr，α，β，p，q，r，L)

N＝f_N(δe，δa，δr，α，β，p，q，r，L)

2)量测方程的建立

观察飞机运动的全量12个微分方程，未找到与机体三轴角速率直接相关的观测量，但得到姿态角速率与机体三轴角速率满足以下关系式：

如果能够得到姿态角的微分信号，将微分信号作为虚拟的观测量，用于构建无迹卡尔曼滤波器的观测方程，则可通过滤波算法完成角速率信号的重构。

微分信号的常规获取途径是进行差分运算，但是在进行差分运算的同时，噪声信号被严重放大，如果利用该信号作为观测量去参与滤波过程，效果可想而知。非线性跟踪微分为该问题提出了一个很好的解决思路——对于含有随机噪声的信号，非线性跟踪微分器能够在一定程度上滤除噪声的影响。下面对非线性跟踪微分器的滤波效果进行验证。

某平直稳态飞行的飞机其俯仰角为θ＝0.0464959rad，假定俯仰角传感器的测量噪声为高斯白噪声，均值为0，方差为1e-10。对于稳态飞行，理论上其俯仰角为恒定值，则俯仰角的微分信号应为0。而实际中传感器的测量噪声是不可避免的，因此实际中的俯仰角微分信号为非零，但越小越好。图2给出的是非线性跟踪微分器与差分计算得到的俯仰角微分信号的对比，验证了非线性跟踪微分器良好的滤波性能，也在一定程度上说明了利用非线性跟踪微分器输出的微分信号作为虚拟观测信号的可行性。

将由捷联惯导系统获取的俯仰角θ、滚转角φ、偏航角ψ分别输入到非线性跟踪微分器得到俯仰角微分

滚转角微分

和偏航角的微分

选取俯仰角微分

滚转角微分和偏航角微分

作为观测变量，即观测向量为

进而建立无迹卡尔曼滤波器量测方程：

Z(t)＝h[X(t)，u(t)，t]+v(t)

量测方程具体可表示如下，v(t)为量测噪声序列，为高斯白噪声。

姿态角速率信号的获取具体如下步骤所示：

由姿态传感器可以测量得到姿态角φ(t)，θ(t)，ψ(t)，同时不可避免的包含测量噪声信号。选择输入为V(t)＝[φ(t) θ(t) ψ(t)]^T，将V(t)输入到下式所描述的离散形式的非线性跟踪微分器，可获得两个输出信号X₁(t)，X₂(t)。其中X₁(t)跟踪输入信号V(t)，X₁(t)相对V(t)其噪声有所削弱；X₂(t)跟踪V(t)的微分信号，即X₂(t)近似等于

$\left\{\begin{array}{c}{X}_1(k+1)=X_1\left(k\right)+h*x_2\left(k\right)\\ X_2(k+1)=X_2\left(k\right)+h*\mathrm{fst}2(X_1\left(k\right),X_2\left(k\right),V\left(k\right),r,h_1)\end{array}\right.$

其中fst2(·)由下式给出：

$\mathrm{fst}2(X_1\left(k\right),X_2\left(k\right),V\left(k\right),r,h_1)=\left\{\begin{array}{c}-\mathrm{rsign}\left(g\left(k\right)\right),|g\left(k\right)\&GreaterEqual;\mathrm{\&delta;}|\\ -\mathrm{rg}\left(k\right)/\mathrm{\&delta;},|g\left(k\right)\&le;\mathrm{\&delta;}|\end{array}\right.$

其中g(k)由下式给出：

$g\left(k\right)=\left\{\begin{array}{c}{X}_2\left(k\right)-\mathrm{sign}\left(z_1\left(k\right)\right)(\mathrm{\&delta;}-\sqrt{{\mathrm{\&delta;}}^2+8r\left|z_1\left(k\right)\right|/2}),\left|z_1\left(k\right)\right|\&GreaterEqual;{\mathrm{\&delta;}}_1\\ X_2\left(k\right)+z_1\left(k\right)/h_1,\left|z_1\left(k\right)\right|\&le;{\mathrm{\&delta;}}_1\end{array}\right.$

δ＝h₁r，δ₁＝h₁δ

e(k)＝X₁(k)-V(k)，z₁(k)＝e(k)-h₁X₂(k)

上述式中，h为数值积分步长，此处等于飞机模型的仿真步长；r决定了跟踪的快慢，称为快慢因子，r越大跟踪越快，但是噪声放大就越厉害；h₁为决定噪声滤波效应的参数，称为滤波因子，h₁越大滤波效果越好，但跟踪相位损失也越大。研究表明：当h₁＞h时，对于含噪声的信号，NTD滤波器才有较好的滤波功能。因此，在确定滤波参数取值时，r和h₁需要协调调整。

3)角速率重构具体实现过程

步骤一)：状态方程和观测方程的离散化

假定离散化时间为T，对状态方程离散化有

X(k+1)＝F(X(k)，u(k)，T)·X(k)+w(k)

即：

$\left[\begin{array}{c}p(k+1)\\ q(k+1)\\ r(k+1)\end{array}\right]=\left[\begin{array}{ccc}1+f_1(\&CenterDot;)\&CenterDot;T& 0& 0\\ 0& 1+f_2(\&CenterDot;)\&CenterDot;T& 0\\ 0& 0& 1+f_3(\&CenterDot;)\&CenterDot;T\end{array}\right]\&CenterDot;\left[\begin{array}{c}p\left(k\right)\\ q\left(k\right)\\ r\left(k\right)\end{array}\right]+\left[\begin{array}{c}{w}_1\left(k\right)\\ w_2\left(k\right)\\ w_3\left(k\right)\end{array}\right]$

对观测方程进行离散化有

Z(k+1)＝h(X(k+1))+v(k+1)

即：

其中φ(k+1)、θ(k+1)、ψ(k+1)分别为(k+1)时刻的滚转角、俯仰角和偏航角测量值，由捷联惯导系统输出。w(k)＝[w₁(k) w₂(k) w₃(k)]^T，v(k)＝[v₁(k) v₂(k) v₃(k)]^T分别为系统激励噪声序列和观测噪声序列，满足：

E[w(k)]＝0，Cov[w(k)，w(j)]＝E[w(k)w^T(j)]＝Q_kδ_kj

E[v(k)]＝0，Cov[v(k)，v(j)]＝E[v(k)v^T(j)]＝R_kδ_kj

Cov[w(k)，v(j)]＝E[w(k)v^T(j)]＝0

式中， ${\left(Q_k\right)}_{3\&times;3}=\left[\begin{array}{ccc}{\mathrm{\&sigma;}}_{w_1\left(k\right)}^2& 0& 0\\ 0& {\mathrm{\&sigma;}}_{w_2\left(k\right)}^2& 0\\ 0& 0& {\mathrm{\&sigma;}}_{w_3\left(k\right)}^2\end{array}\right],{\left(R_k\right)}_{3\&times;3}=\left[\begin{array}{ccc}{\mathrm{\&sigma;}}_{v_1\left(k\right)}^2& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_2\left(k\right)}^2& 0\\ 0& 0& {\mathrm{\&sigma;}}_{v_3\left(k\right)}^2\end{array}\right]$ 分别为激励噪声矢量的协方差矩阵和观测噪声矢量的协方差矩阵，Q_k为非负定矩阵，R_k为正定矩阵。

为相关噪声矢量的标准方差常数。δ为狄拉克函数，满足

$\left\{\begin{array}{cc}\mathrm{\&delta;}(x-c)=0& x\&NotEqual;c\\ {\&Integral;}_a^b\mathrm{\&delta;}(x-c)=1& a<c<b\end{array}\right.$

步骤二)：初始化操作

假定三轴角速率的初始估计值 ${\hat{X}}_0={\left[\begin{array}{ccc}{p}_0& q_0& r_0\end{array}\right]}^T,$ 初始条件下的估计均方误差阵为 $P_{k0}=\mathrm{diag}\left[\begin{array}{ccc}{\mathrm{\&sigma;}}_{p0}^2& {\mathrm{\&sigma;}}_{q0}^2& {\mathrm{\&sigma;}}_{r0}^2\end{array}\right],$

分别是滚转角速率，俯仰角速率，偏航角速率的初始估计方差。

选择对称采样策略，则相应的均值加权值W_k ^(m)和方差加权值W_k ^(c)分别可表示为：

${W_0}^{\left(m\right)}=\frac{\mathrm{\&lambda;}}{n+\mathrm{\&lambda;}}$

${W_0}^{\left(c\right)}=\frac{\mathrm{\&lambda;}}{n+\mathrm{\&lambda;}}+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})$

${W_i}^{\left(m\right)}={W_i}^{\left(c\right)}=\frac{1}{2\&CenterDot;(n+\mathrm{\&lambda;})},i=1,L2n$

其中n为状态维数，本文中n＝3；λ＝α²(n+κ)-n为尺度因子，κ用于保证方差矩阵的半正定性，一般取κ＝0或κ＝3-n，其取值大小对算法影响并不大；α为比例修正参数(常取1e-4≤α＜1)，以避免在系统非线性较强时的非局部采样。β用于引入状态先验分布的高阶项信息，取值范围β≥0，对于高斯分布，β＝2最优；对于非高斯分布，该参数还具有控制误差的作用，可以控制后验分布拖尾的大小。

步骤三)：系统状态Sigma点采样

根据k-1时刻的系统状态估计值

和协方差矩阵P_k-1|k-1进行Sigma点采样。对称采样Sigma点样本个数为L＝2n+1＝7个，则对应于k-1时刻的Sigma采样点为

${\mathrm{\&chi;}}_{k-1}=\left[\begin{array}{ccc}{\hat{X}}_{k-1|k-1}& {\hat{X}}_{k-1|k-1}-\sqrt{(L+\mathrm{\&lambda;})P_{k-1|k-1}}& {\hat{X}}_{k-1|k-1}+\sqrt{(L+\mathrm{\&lambda;})P_{k-1|k-1}}\end{array}\right]$

步骤四)：UKF时间更新

根据离散化的状态方程式，对上述采样的7个Sigma点进行状态预测，则有

χ_i，k，k-1＝F(χ_i，k-1)     i＝0，1L，6

利用上述采样预测值确定系统状态向量和协方差矩阵的最终预测值为：

${\hat{X}}_{k|k-1}=\underset{i=0}{\overset{6}{\mathrm{\&Sigma;}}}{W_i}^m\&CenterDot;{\mathrm{\&chi;}}_{i,k|k-1}$

$P_{k|k-1}=\underset{i=0}{\overset{6}{\mathrm{\&Sigma;}}}{W_i}^c({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1}){({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1})}^T+Q_k$

η_k|k-1＝h(χ_k|k-1)

${\hat{Z}}_{k|k-1}=\underset{i=0}{\overset{6}{\mathrm{\&Sigma;}}}{W_i}^m\&CenterDot;{\mathrm{\&eta;}}_{i,k|k-1}$

步骤五)：UKF量测更新

$P_{\mathrm{ZZk}|k-1}=\underset{i=0}{\overset{6}{\mathrm{\&Sigma;}}}{W_i}^{\left(c\right)}({\mathrm{\&eta;}}_{i,k|k-1}-{\hat{Z}}_{k|k-1})\&CenterDot;{({\mathrm{\&eta;}}_{i,k|k-1}-{\hat{Z}}_{k|k-1})}^T+R_k$

$P_{\mathrm{XZk}|k-1}=\underset{i=0}{\overset{6}{\mathrm{\&Sigma;}}}{W_i}^{\left(c\right)}({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1})\&CenterDot;{({\mathrm{\&eta;}}_{i,k|k-1}-{\hat{Z}}_{k|k-1})}^T$

K_k＝P_XZk|k-1·(P_ZZk|k-1)^-1

${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k(Z_k-{\hat{Z}}_{k|k-1})$

$P_k=P_{k|k-1}-K_k{P}_{\mathrm{ZZk}|k-1}{K}_k^T$

其中的为k时刻的系统观测值，由非线性跟踪微分器输出获得。

上述算法过程可以进一步概括为：首先根据系统状态的统计特性

和P_k0，选择一种采样策略得到相应的Sigma点集，现今广泛使用的Sigma点采样策略主要包括对称采样、最小偏度单形采样、超球面单形采样、比例修正采样、高斯分布4阶矩对称采样以及3阶矩偏度采样等，本文选择比例对称采样策略得到Sigma采样点集；将采样得到的Sigma点集通过非线性的状态方程进行传播，得到变换后的Sigma点集；对变换后的Sigma点集进行相应的加权处理，得到状态的一步预测值以及一步预测均方误差矩阵。然后将经过非线性变换后的Sigma点集通过非线性的量测方程进行传播并加权处理，得到观测量的一步预测值

协方差矩阵P_ZZ，k|k-1以及滤波增益矩阵K_k。利用测量值Z_k与观测量的一步预测值

以及滤波增益K_k去修正状态的一步预测

从而得到状态的估计值

滤波过程得以完成，最终得到飞机三轴角速率的重构信号。

4)角速率信号重构

使用非线性的飞机模型进行数字仿真验证，飞机的初始速度为V＝150.148m/s，初始滚转角速率为p＝-1.3°/s，q＝0.14°/s，r＝1.16°/s；采样周期为T＝0.05s；非线性跟踪微分器的仿真步长为h＝0.05s，滤波因子h₁＝0.06s，快慢因子r＝12。

以高斯白噪声模拟陀螺仪的测量误差，Q，R分别为过程噪声和测量噪声方差阵，且有

Q＝10^-4*diag([1 1 1])

R＝10^-6*diag([1 1 1])

基于上述仿真条件，对滤波得到的数据以及无噪声的理论飞机飞行状态值在50s内进行采样，可得基于UKF和NTD的飞行状态估计结果如图4-图9所示。

从图4-图9的仿真曲线可以看出，基于UKF和NTD的方法所得的估计信号与真实信号之间的偏差虽存在一定的波动，但仍具有很好的估计效果。为了验证算法的一般有效性，在仿真过程中给三个角速率信号加入了一定幅值的阶跃指令输入，估计信号可以快速的跟踪上真实信号并且误差也在允许的范围之内，说明算法具有很好的实时性和鲁棒性。此外，仿真是在实时环境下进行的，该方法又具有很好的实时性。

以上所述，仅为本发明较佳的具体实施方式，本发明的保护范围不限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可显而易见地得到的技术方案的简单变化或等效替换均落入本发明的保护范围内。

Claims (2)

1.一种基于无迹卡尔曼滤波的飞机角速率信号重构方法，其特征在于，包括以下步骤：

(1)以周期T读取捷联惯导系统中的三个姿态角信息，三个姿态角分别为俯仰角θ、滚转角φ、偏航角ψ；

(2)以周期T读取飞行控制系统输出的飞机所受到的合外力矩，合外力矩在机体坐标系下x轴、y轴和z轴的分量分别为滚转力矩

俯仰力矩M和偏航力矩N；

(3)利用飞机的转动惯量(I_x，I_y，I_z)和惯性积I_xz按照下式计算力矩方程组的系数

$c_1=\frac{(I_y-I_z)I_z-{I_{\mathrm{xz}}}^2}{\mathrm{\&Sigma;}},$ $c_2=\frac{(I_x-I_y+I_z)I_{\mathrm{xz}}}{\mathrm{\&Sigma;}},$ $c_3=\frac{I_z}{\mathrm{\&Sigma;}},$ $c_4=\frac{I_{\mathrm{xz}}}{\mathrm{\&Sigma;}},$ $c_5=\frac{I_z-I_x}{I_y},$ $c_6=\frac{I_{\mathrm{xz}}}{I_y},$ $c_7=\frac{1}{I_y},$ $c_8=\frac{(I_x-I_y)I_x-{I_{\mathrm{xz}}}^2}{\mathrm{\&Sigma;}},$ $c_9=\frac{I_x}{\mathrm{\&Sigma;}},$ ∑＝I_xI_z-I_xz ²；

(4)将步骤(1)获取的飞机的俯仰角θ、滚转角φ、偏航角ψ分别输入到非线性跟踪微分器得到俯仰角微分

滚转角微分

和偏航角的微分

$\left\{\begin{array}{c}{x}_1(k+1)=x_1\left(k\right)+h*x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)+h*\mathrm{fst}2(x_1\left(k\right),x_2\left(k\right),\mathrm{vv}\left(k\right),r,h_1)\end{array}\right.$

上式中将含测量噪声的θ、φ和ψ分别代替vv(k)，则相应的x₂(k)分别对应于

和

其中h为离散型非线性跟踪微分器的仿真步长，这里取h＝T，r和h₁分别为滤波因子和快慢因子；

(5)根据飞行动力学模型，选取飞机的俯仰角速率q、滚转角速率p和偏航角速率r作为状态变量，即状态向量X＝[p q r]^T，进而建立无迹卡尔曼滤波器的状态方程；选取步骤(4)得到的俯仰角微分滚转角微分

和偏航角微分

作为观测变量，即观测向量为

进而建立无迹卡尔曼滤波器量测方程；

(6)根据步骤(1)和步骤(4)获取的当前时刻即t_k+1时刻的量测信息，步骤(2)获取的上一时刻即t_k时刻的合外力矩，步骤(3)计算得到的9个力矩方程组系数，利用无迹卡尔曼滤波方程得到t_k+1时刻状态量的最优估计值，从而实现t_k+1时刻的三轴角速率信号的实时精确估计；

(7)将步骤(6)得到的t_k+1时刻的角速率信号的精确估计值反馈给无迹卡尔曼滤波算法模块，用于完成步骤(6)中的下一时刻即t_k+2时刻角速率信号的估计。

2.根据权利要求1所述的基于无迹卡尔曼滤波的飞机角速率信号重构方法，其特征在于，所述步骤(6)中无迹卡尔曼滤波算法的具体步骤是：

(a)无迹卡尔曼滤波器状态方程的建立

根据飞行动力学模型，选取飞机的俯仰角速率q、滚转角速率p和偏航角速率r作为状态变量，即状态向量X＝[p q r]^T，进而建立无迹卡尔曼滤波器的状态方程：

状态方程的具体形式为：

其中，u(t)为控制输入量，包括升降舵偏角δe，副翼偏转角δa和方向舵偏角δr，滚转力矩俯仰力矩M以及偏航力矩由下式计算得到

$\stackrel{\&OverBar;}{L}=f_{\stackrel{\&OverBar;}{L}}(\mathrm{\&delta;e},\mathrm{\&delta;a},\mathrm{\&delta;r},\mathrm{\&alpha;},\mathrm{\&beta;},p,q,r,L)$

M＝f_M(δe，δa，δr，α，β，p，q，r，L)

N＝f_N(δe，δa，δr，α，β，p，q，r，L)

w(t)为激励噪声序列，这里取为高斯白噪声；

(b)无迹卡尔曼滤波器量测方程的建立

将由捷联惯导系统获取的俯仰角θ、滚转角φ、偏航角ψ分别输入到非线性跟踪微分器得到俯仰角微分

滚转角微分

和偏航角的微分

其中非线性跟踪微分器的离散形式为：

$\left\{\begin{array}{c}{x}_1(k+1)=x_1\left(k\right)+h*x_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)+h*\mathrm{fst}2(x_1\left(k\right),x_2\left(k\right),\mathrm{vv}\left(k\right),r,h_1)\end{array}\right.$

vv(k)为含噪声的输入信号，x₁(k)用于跟踪输入信号vv(k)，而x₂(k)则跟踪vv(k)的微分信号，在上式中将含测量噪声的θ、φ和ψ分别代替vv(k)，则相应的x₂(k)分别对应于

和

其中h为离散型非线性跟踪微分器的仿真步长，这里取h＝T，r和h₁分别为滤波因子和快慢因子，

选取俯仰角微分滚转角微分

和偏航角微分

作为观测变量，即观测向量为

进而建立无迹卡尔曼滤波器量测方程：

Z(t)＝h[X(t)，u(t)，t]+v(t)

量测方程具体可表示为：

v(t)为量测噪声序列，同样选择为高斯白噪声；

(c)状态方程和观测方程的离散化

假定离散化时间为T，对状态方程离散化有

X(k+1)＝F(X(k)，u(k)，T)·X(k)+w(k)

即：

$\left[\begin{array}{c}p(k+1)\\ q(k+1)\\ r(k+1)\end{array}\right]=\left[\begin{array}{ccc}1+f_1(\&CenterDot;)\&CenterDot;T& 0& 0\\ 0& 1+f_2(\&CenterDot;)\&CenterDot;T& 0\\ 0& 0& 1+f_3(\&CenterDot;)\&CenterDot;T\end{array}\right]\&CenterDot;\left[\begin{array}{c}p\left(k\right)\\ q\left(k\right)\\ r\left(k\right)\end{array}\right]+\left[\begin{array}{c}{w}_1\left(k\right)\\ w_2\left(k\right)\\ w_3\left(k\right)\end{array}\right]$

对观测方程进行离散化有

Z(k+1)＝h(X(k+1))+v(k)

即：

其中φ(k+1)、θ(k+1)、ψ(k+1)分别为(k+1)时刻的滚转角、俯仰角和偏航角测量值，由捷联惯导系统输出，w(k)＝[w₁(k) w₂(k) w₃(k)]^T，v(k)＝[v₁(k) v₂(k) v₃(k)]^T分别为系统激励噪声序列和观测噪声序列，满足：

E[w(k)]＝0，Cov[w(k)，w(j)]＝E[w(k)w^T(j)]＝Q_kδ_kj

E[v(k)]＝0，Cov[v(k)，v(j)]＝E[v(k)v^T(j)]＝R_kδ_kj

Cov[w(k)，v(j)]＝E[w(k)v^T(j)]＝0

式中， ${\left(Q_k\right)}_{3\&times;3}=\left[\begin{array}{ccc}{\mathrm{\&sigma;}}_{w_1\left(k\right)}^2& 0& 0\\ 0& {\mathrm{\&sigma;}}_{w_2\left(k\right)}^2& 0\\ 0& 0& {\mathrm{\&sigma;}}_{w_3\left(k\right)}^2\end{array}\right],$ ${\left(R_k\right)}_{3\&times;3}=\left[\begin{array}{ccc}{\mathrm{\&sigma;}}_{v_1\left(k\right)}^2& 0& 0\\ 0& {\mathrm{\&sigma;}}_{v_2\left(k\right)}^2& 0\\ 0& 0& {\mathrm{\&sigma;}}_{v_3\left(k\right)}^2\end{array}\right]$ 分别为激励噪声矢量的协方差矩阵和观测噪声矢量的协方差矩阵，Q_k为非负定矩阵，R_k为正定矩阵，

为相关噪声矢量的标准方差常数，δ为狄拉克函数，满足

$\left\{\begin{array}{cc}\mathrm{\&delta;}(x-c)=0& x\&NotEqual;c\\ {\&Integral;}_a^b\mathrm{\&delta;}(x-c)=1& a<c<b\end{array}\right.;$

(d)无迹卡尔曼滤波算法

假定三轴角速率的初始估计值 ${\hat{X}}_0={\left[\begin{array}{ccc}{p}_0& q_0& r_0\end{array}\right]}^T,$ 初始条件下的估计均方误差阵为 $P_{k0}=\mathrm{diag}\begin{array}{c}\left[\begin{array}{ccc}{\mathrm{\&sigma;}}_{p0}^2& {\mathrm{\&sigma;}}_{q0}^2& {\mathrm{\&sigma;}}_{r0}^2\end{array}\right],\end{array}$

分别是滚转角速率，俯仰角速率，偏航角速率的初始估计方差，

选择对称采样策略，则相应的均值加权值W_k ^(m)和方差加权值W_k ^(c)分别可表示为：

${W_0}^{\left(m\right)}=\frac{\mathrm{\&lambda;}}{n+\mathrm{\&lambda;}}$

${W_0}^{\left(c\right)}=\frac{\mathrm{\&lambda;}}{n+\mathrm{\&lambda;}}+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})$

${W_i}^{\left(m\right)}={W_i}^{\left(c\right)}=\frac{1}{2\&CenterDot;(n+\mathrm{\&lambda;})},i=1,L2n$

其中n为状态维数，本文中n＝3；λ＝α²(n+κ)-n为尺度因子，κ用于保证方差矩阵的半正定性，一般取κ＝0或κ＝3-n，其取值大小对算法影响并不大；α为比例修正参数(常取1e-4≤α＜1)，以避免在系统非线性较强时的非局部采样，β用于引入状态先验分布的高阶项信息，取值范围β≥0，对于高斯分布，β＝2最优；对于非高斯分布，该参数还具有控制误差的作用，可以控制后验分布拖尾的大小，

系统状态Sigma点采样

根据k-1时刻的系统状态估计值

和协方差矩阵P_k-1|k-1进行Sigma点采样，对称采样Sigma点样本个数为L＝2n+1＝7个，则对应于k-1时刻的Sigma采样点为

${\mathrm{\&chi;}}_{k-1}=\left[\begin{array}{ccc}{\hat{X}}_{k-1|k-1}& {\hat{X}}_{k-1|k-1}-\sqrt{(L+\mathrm{\&lambda;})P_{k-1|k-1}}& {\hat{X}}_{k-1|k-1}+\sqrt{(L+\mathrm{\&lambda;})P_{k-1|k-1}}\end{array}\right]$

UKF时间更新

根据离散化的状态方程式，对上述采样的7个Sigma点进行状态预测，则有

χ_i，k，k-1＝F(χ_i，k-1)     i＝0，1L，6

利用上述采样预测值确定系统状态向量和协方差矩阵的最终预测值为：

${\hat{X}}_{k|k-1}=\underset{i=0}{\overset{6}{\mathrm{\&Sigma;}}}{W_i}^m\&CenterDot;{\mathrm{\&chi;}}_{i,k|k-1}$

$P_{k|k-1}=\underset{i=0}{\overset{6}{\mathrm{\&Sigma;}}}{W_i}^c({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1}){({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1})}^T+Q_k$

η_k|k-1＝h(χ_k|k-1)

${\hat{Z}}_{k|k-1}=\underset{i=0}{\overset{6}{\mathrm{\&Sigma;}}}{W_i}^m\&CenterDot;{\mathrm{\&eta;}}_{i,k|k-1}$

UKF量测更新

$P_{\mathrm{ZZk}|k-1}=\underset{i=0}{\overset{6}{\mathrm{\&Sigma;}}}{W_i}^{\left(c\right)}({\mathrm{\&eta;}}_{i,k|k-1}-{\hat{Z}}_{k|k-1})\&CenterDot;{({\mathrm{\&eta;}}_{i,k|k-1}-{\hat{Z}}_{k|k-1})}^T+R_k$

$P_{\mathrm{XZk}|k-1}=\underset{i=0}{\overset{6}{\mathrm{\&Sigma;}}}{W_i}^{\left(c\right)}({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1})\&CenterDot;{({\mathrm{\&eta;}}_{i,k|k-1}-{\hat{Z}}_{k|k-1})}^T$

K_k＝P_XZk|k-1·(P_ZZk|k-1)^-1

${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k(Z_k-{\hat{Z}}_{k|k-1})$

$P_k=P_{k|k-1}-K_k{P}_{\mathrm{ZZk}|k-1}{K}_k^T$

其中的

为k时刻的系统观测值，由非线性跟踪微分器输出获得。

Images