[发明专利]一种角速度辅助的Kalman滤波定位方法

权利要求书

1.一种角速度辅助的Kalman滤波定位方法，其特征在于：包括如下步骤：

步骤1、运动载体的接收机捕获并跟踪卫星信号，解调卫星导航电文，剔除故障卫星和异常卫星；

步骤2、利用接收到的前几个历元的卫星信息解算运动载体的初始状态变量X₀、方位角S₀、角速度Vr₀、方差P₀；

步骤3、确定采用的Kalman滤波方程模型，假设没有系统控制输入，根据不同载体类型和不同的接收机类型确定以下参数：过程激励噪声协方差矩阵为Q，观测噪声协方差矩阵为R，转换矩阵Φ，测量矩阵H，马尔可夫过程在三个坐标轴方向的相关时间常数τ_x、τ_y、τ_z；

取状态变量为X＝[x v_x a_x ε_x y v_y a_y ε_y z v_z a_z ε_z]^T，观测变量Z＝[L_x L_vx L_y L_vy L_zL_vz]^T；

其中，分别为X、Y、Z轴上的加速度方差矩阵，分别为X、Y、Z轴上的定位误差方差矩阵；其中(x v_x a_x)、(y v_y a_y)、(z v_z a_z)3组状态变量分别为xyz三个坐标轴上的位置、速度、加速度分量，ε_x、ε_y、ε_z分别为各种误差源在三个坐标轴方向造成的总位置误差，观测变量(L_x L_vx)、(L_y L_vy)、(L_z L_vz)分别为接收机解算出的xyz三个坐标轴上的位置、速度分量；

R＝diag[R_x R_y R_z]，其中R_x、R_y、R_z分别为X、Y、Z轴上的观测噪声协方差分量；

步骤4、根据现时刻接收卫星信息初步解算位置、速度、载体运行方位角，然后根据前一时刻的位置、速度、加速度，利用卡尔曼滤波方程解算现时刻的位置、速度和加速度；

具体包括如下步骤：

步骤401、检测接收卫星信息是否良好以解算当前载体运行方位角，若是，利用如下卡尔曼滤波方程解算当前时刻的状态变量返回退出；若否，转到步骤402；

K_k＝P_(k，k-1)H^T(HP_(k，k-1)H^T+R)^-1

P_(k，k-1)＝ΦP_k-1Φ^T+Q

P_k＝(I-K_kH)P_(k，k-1)

式中，是利用K-1时刻状态变量预测的K时刻状态变量预测结果，是K-1时刻的状态变量最优结果，是K时刻的状态变量最优估算值，K_k为卡尔曼增益；P_k为后验估计误差的协方差矩阵，P_(k，k-1)为先验估计误差的协方差矩阵，Z_k是K时刻观测变量；

步骤402、将k-1时刻状态变量X_k-1＝[x，v_x，a_x，ε_x，y，v_y，a_y，ε_y，z，v_z，a_z，ε_z]^T由XYZ坐标系转换为NEU坐标系，设NEU坐标系下的速度变量为Vn0、Ve0、Vh0：

步骤403、计算添加角速度辅助后的速度变量：

Vn＝(Vn0*Vn0+Ve0*Ve0)^1/2*cos(S_k-1)

Ve＝(Vn0*Vn0+Ve0*Ve0)^1/2*sin(S_k-1)

Vh＝Vh0

步骤404、令Vr_k-1＝Vr_k-2，Vr_k-1、Vr_k-2分别是K-1、K-2时刻的角速度；根据辅助方程角速度：Vr_k-1＝(S_k-S_k-1)/T计算S_k＝S_k-1+Vr_k-1*T，S_k是K时刻的载体运行方位角，T为观测时间间隔；步骤405、将NEU坐标系下的速度变量Vn、Ve、Vh转换为XYZ坐标系，设此时XYZ坐标系下的速度变量相应为v_x1、v_y1、v_z1，令

步骤406、联合Kalman滤波方程，两者进行等权计算现时刻状态变量预测结果：返回退出。