[发明专利]车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法

权利要求书

1.车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法，其特征在于，包括以下步骤：

步骤一：建立组合导航系统的状态方程与量测方程

x_k＝Φ_k/k-1x_k-1+w_k-1

z_k＝H_kx_k+v_k

x_k为状态矩阵，z_k为测量矩阵，Φ_k/k-1为状态转移矩阵，H_k为量测转移矩阵；w_k-1为系统高斯白噪声，系统噪声方差为Q_k-1；v_k测量高斯白噪声，量测噪声方差为R_k；状态方程与测量方程具体为：

以姿态误差的微分、速度误差的微分、经纬度误差的微分、陀螺仪误差以及加速度计误差作为状态变量，利用GPS系统提供的位置信息与速度信息与微惯性导航提供的位置与速度信息的差值作为观测变量，

状态变量为

量测变量为

状态转移矩阵具体元素如下，其中第一个数字为行，第二个数字为列：

φ(1,2)＝w_U+v_EtanL/R_Nh φ(1,3)＝w_U+v_E/R_Nh

φ(1,5)＝-1/R_Mh

φ(2,1)＝-(w_U+v_EtanL/R_Nh) φ(2,3)＝-v_N/R_Mh

φ(2,4)＝1/R_Nh φ(2,7)＝-w_U

φ(3,1)＝w_N+v_E/R_Nh φ(3,2)＝v_N/R_Mh

φ(3,4)＝tanL/R_Nh φ(3,7)＝w_N+v_Esec²L/R_Nh

φ(4,4)＝(v_EtanL-v_U)/R_Nh

φ(4,5)＝2w_U+v_EtanL/R_Nh φ(4,6)＝-(2w_N+v_E/R_Nh)

φ(4,7)＝2(v_Nw_N+v_Uw_U)+v_Ev_Nsec²L/R_Nh

φ(5,4)＝-2(w_U+v_EtanL/R_Nh)

φ(5,5)＝-v_U/R_Mh φ(5,6)＝-v_N/R_Mh

φ(5,7)＝-v_E(2w_N+v_Esec²L/R_Nh)

φ(6,4)＝2(w_N+v_E/R_Nh) φ(6,5)＝v_N/R_Mh

φ(6,7)＝-2w_Uv_E

φ(7,5)＝1/R_Mh

φ(8,4)＝secL/R_Nh φ(8,7)＝v_EsecLtanL/R_Nh

φ(9,6)＝1

量测系数矩阵为：

其中分别代表俯仰角误差的微分、横滚角误差的微分、航向角误差的微分，分别代表东向速度误差的微分、北向速度误差的微分、天向速度误差的微分，分别代表经度误差的微分、纬度误差的微分、高度误差的微分， ν_Eν_Nν_U代表东向速度、北向速度、天向速度，Lλh代表经度、纬度与高度；ε代表陀螺仪误差，代表加速度计误差；R_Mh、R_Nh分别代表子午面、卯酉面地球半径；为地球自转角速度；代表载体系到地理坐标系的转换矩阵；f^b代表加速度计测得的比力信息， 0_n×m代表n列m行的零矩阵，I_n×m代表n列m行的单位矩阵；

步骤二：采用一种基于噪声方差的自适应参数估计方法进行噪声参数估计，其具体实施过程为：首先进行状态一步预测得到X_k/k-1＝Φ_k/k-1X_k-1以及Z_k/k-1＝H_kX_k/k-1；其次计算量测预测误差e_z,k/k-1＝Z_k-Z_k/k-1与状态估计误差e_x,k/k-1＝X_k-X_k/k-1以及调节参数β_Q,k、β_R,k，再次利用R_k与Q_k-1的递推更新公式进行测量噪声方差R_k与系统噪声方差Q_k-1的更新；然后利用序贯滤波，限制R_k与Q_k-1中对角元素的大小，保证R_k与Q_k-1的正定性，防止滤波发散；最后将得到的量测噪声方差R_k与系统噪声方差Q_k-1代入到卡尔曼滤波器增益计算方程与状态更新方程中，完成对状态误差的估计，得到最优状态误差；对量测噪声方差R_k与系统噪声方差Q_k-1进行计算与更新时的自适应计算方法为：

首先计算状态一步预测：X_k/k-1＝Φ_k/k-1X_k-1，以及量测一步预测：Z_k/k-1＝H_kX_k/k-1

然后得到量测预测误差e_z,k/k-1＝Z_k-Z_k/k-1与状态估计误差e_x,k/k-1＝X_k-X_k/k-1，

在噪声平稳的条件下，以时间指数渐消记忆加权平均方法代替集总平均，并转化为递推算法，噪声递推公式如下：

R_k＝(1-β_R,k)R_k-1+β_R,k(e_z,k/k-1e_z,k/k-1^T-H_kP_k/k-1H_k^T)

Q_k-1＝(1-β_Q,k)Q_k-2+β_Q,k(K_ke_y,k/k-1K_k^T+P_k-Φ_k/k-1P_k-1Φ_k/k-1^T)

以及参数递推公式：

初值β_R,0β_Q,0＝1，渐消因子b₁与b₂分别取0.98、0.95；P_k代表k时刻的状态估计均方误差，P_k/k-1代表一步预测均方误差。利用以上计算方法可以对量测噪声方差R_k与系统噪声方差Q_k-1进行计算与更新；

最后将得到的量测噪声方差R_k与系统噪声方差Q_k-1带入到卡尔曼滤波器增益计算方程与状态更新方程中，增益计算方程为K_k＝(Φ_k/k-1P_k-1Φ_k/k-1^T+Q_k-1)H_k^T(H_k/k-1P_k-1H_k/k-1^T+R_k)，状态更新方程为：X_k＝X_k/k-1+K_k(Z_k-(H_kX_k/k-1+v_k))，得到最优状态误差；对量测噪声方差R_k与系统噪声方差Q_k-1进行计算与更新时，序贯滤波方法为：

标量量测方程为：

记为

利用上限条件与下限条件将测量噪声限定在合理的范围内，具体如下：

Q_k-1计算如下：

在以上公式中，上标i代表的是相应矩阵中的第i行标量。X_k为状态矩阵，Z_k为测量矩阵，Φ_k/k-1为状态转移矩阵，H_k为量测转移矩阵，Q_k-1为系统噪声方差；v_k为测量高斯白噪声，R_k为量测噪声方差；P_k代表k时刻的状态估计均方误差，P_k/k-1代表一步预测均方误差， R_max＝100×R₀，R_min＝0.01×R₀，Q_max＝100×Q₀，Q_min＝0.01×Q₀，R₀为量测噪声方差初值，Q₀为系统噪声方差初值；

步骤三：采用松组合导航方法，利用前面得到的最优状态误差去校正微惯性导航系统的输出，获得车辆姿态角、速度与位置的最优值。

2.如权利要求1要求的车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法，其特征在于,所述步骤三中利用前面得到的最优状态误差去校正微惯性导航系统输出的计算方法为:

姿态角修正att＝att_INS-φ_E，速度修正V＝V_INS-δV，位置修正P＝P_INS-δP其中att_INS、V_INS、P_INS为MINS解算出的姿态信息、速度信息、位置信息，φ_E、δV、δP为卡尔曼滤波估计出的误差最优值；att、V、P分别为修正后的姿态信息、速度信息、位置信息，将其作为输出。