[发明专利]一种基于卡尔曼滤波的QAR数据噪声处理方法

权利要求书

1.一种基于卡尔曼滤波的QAR数据噪声处理方法，其特征在于，包括以下步骤：

步骤1，定义t时刻状态向量、t时刻状态转移矩阵、t时刻输入传递矩阵；

步骤2，将t时刻飞机状态向量和t时刻实际状态转移协方差矩阵结合状态转移矩阵、输入传递矩阵进行计算，得到t+1时刻预测状态向量、t+1时刻飞机状态预测误差协方差矩阵、t+1时刻转移状态误差协方差矩阵；

步骤3，将t+1时刻状态向量观测值与t+1时刻观测值向量，根据t+1时刻转移状态误差协方差矩阵得到t+1时刻卡尔曼系数；

步骤4，将于步骤2得到的t+1时刻预测状态向量与观测状态通过t+1时刻卡尔曼系数进行融合得到滤波后的t+1时刻状态，并根据状态误差协方差矩阵得到t+1时刻实际状态转移协方差矩阵；

步骤5，重复步骤2到步骤4，如此循环往复直到所有时刻的状态完成处理，得到完整的航迹数据；

步骤1所述t时刻状态向量为：

其中，State_t为t时刻的状态向量，t用来指代数据的序号，K代表最大序号，与采样次数即数据长度相等，lngt为t时刻飞机所在的经度，lat_t为时刻t飞机所在的纬度，v_t为时刻t飞机相对于地面的速度，当t＝0时，State_t为初始状态向量；

步骤1所述t时刻状态转移矩阵为：

F_t表示t时刻的状态转移矩阵，当t＝0时，F_t为初始状态转移矩阵；其中，ω_x^t表示在t时刻把飞行距离转化成经度需要乘上的系数，ω_y表示把飞行距离转化成纬度需要乘上的系数，θ_t为t时刻飞机的朝向角，△t为QAR数据的采样间隔，上述ω_x^t，ω_y为：

其中，Lat_t表示t时刻飞机所处的纬度，Earth_Radius_Long表示地球长半轴半径，Earth_Radius_Short表示地球短半轴半径；

步骤1所述t时刻输入传递矩阵为：

其中，B_t为t时刻的输入传递矩阵，ω_x^t表示在t时刻把飞行距离转化成经度需要乘上的系数，ω_y表示把飞行距离转化成纬度需要乘上的系数，由于在对轨迹进行采样时△t足够小，根据牛顿运动学定则，此时在做近似匀加速直线运动的状态转移方程为：

X_t+1＝X_t+(V_t*△t+0.5*a_t*△t²)*Sin(θ_t)*ω_x^t

Y_t+1＝Y_t+(V_t*△t+0.5*a_t*△t²)*Cos(θ_t)*ω_y

V_t+1＝V_t+a_t*△t

其中，a_t表示t时刻飞机的水平加速度，V_t*△t+0.5*a_t*△t²表示△t内飞机的水平位移，V_t为t时刻飞机的水平速度，△t为离散时间的采样间隔；

步骤2所述t+1时刻预测状态向量为：

State_t+1^-＝F_t*State_t+B_t*a_t+x_t，0≤t≤K-1

式中，F_t为t时刻的状态转移矩阵，B_t为t时刻的输入传递矩阵，a_t为t时刻飞机的水平加速度，F_t*State_t表示飞机在两个时刻间的状态转移过程，B_t*a_t表示水平加速度对飞机状态的影响；x_t为t时刻系统状态向量误差，该误差使得飞机动力装置对飞机的控制与记录数据之间存在偏差，符合正态分布x_t～N(0，Qt)，Q_t为t时刻状态向量误差的协方差矩阵；

步骤2所述t+1时刻飞机状态预测误差协方差矩阵为：

Cov(State_t+1)＝Cov(F_t*State_t)＝F_tP_tF_t^T，0≤t≤K-1

步骤2中所述t+1时刻转移状态误差协方差矩阵为：

P_t+1^-＝F_tP_tF_t^T+Q_t，0≤t≤K-1

式中，P_t+1^-为t+1时刻的转移状态误差协方差矩阵，P_t为t时刻的状态误差协方差矩阵，F_t为t时刻的状态转移矩阵，Q_t为t时刻状态向量误差的协方差矩阵；

步骤3所述t+1时刻状态向量观测值为：

Z_t+1＝H*State_t+1^-+R，0≤t≤K-1

其中，Z_t+1为t+1时刻状态向量观测值，H为变换矩阵，R为观测误差协方差矩阵；

步骤3所述t+1时刻观测值向量为：

其中，Lng_t+1为t+1时刻观测到的飞机所在经度，Lat_t+1为t+1时刻观测到的飞机所在纬度；

步骤3所述t+1时刻卡尔曼系数为：

K_t+1＝P_t+1^-*H^T*(H*P_t+1^-*H^T+R)^-1，0≤t≤K-1

其中，P_t+1^-为t+1时刻转移状态误差协方差矩阵，H为变换矩阵，R为观测误差协方差矩阵；卡尔曼系数的主要作用是衡量根据t+1时刻转移状态误差协方差矩阵P_t+1^-和观测误差协方差矩阵R的大小，决定卡尔曼滤波得到的估计值是更偏向于预测值还是更偏向观测值；

步骤4所述滤波后的t+1时刻状态为：

State_t+1＝State_t+1^-+K_t+1*(Z_t+1-H*State_t+1^-)，0≤t≤K-1

式中，State_t+1^-为t+1时刻的预测状态向量，Z_t+1为t+1时刻状态向量观测值，H为变化矩阵，K_t+1为t+1时刻的卡尔曼系数；Z_t+1-H*State_t+1^-为实际的观测与预测值之间的残差，其与K_t+1的乘积用来修正飞机位置的预测值；

步骤4所述t+1时刻实际状态转移协方差矩阵为：

P_t+1＝(I-K_t*H)*P_t+1^-，0≤t≤K-1

其中，I为单位矩阵，K_t为t时刻的卡尔曼系数，P_t+1^-为t+1时刻转移状态误差协方差矩阵，H为变换矩阵；随着迭代的进行，最佳状态估计值的噪声分布情况需要不断地进行更新；状态的不确定性随着迭代的进行总体上会逐渐变小，但是由于传递噪声的存在，状态的不确定性又会呈现出逐渐变大的趋势；卡尔曼滤波算法就是通过不断地调整从而在这一状态中达到一个平衡。