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

摘要

一种角速度辅助的Kalman滤波定位方法，包括如下步骤：步骤1、捕获卫星信号，解调导航电文，剔除故障卫星；步骤2、解算运动载体的初始状态变量、方位角、角速度、方差；步骤3、确定采用的Kalman滤波方程模型，根据不同载体类型和不同的接收机类型确定各个参数的值：步骤4、检测接收卫星信息是否良好，若是，直接利用Kalman方程解算出当前时刻的状态变量‑位置、速度和加速度；当接收卫星信息不好，不能利用卫星信息进行卫星定位时，确定现时刻的载体的所在方位，联合Kalman滤波的预测方程，两者进行等权计算现时刻状态解算载体的位置、速度等信息。本发明在卫星定位失败时，能够显著提高没有惯性传感器等辅助手段情况下的动态定位精度。

主权项

一种角速度辅助的Kalman滤波定位方法，其特征在于：包括如下步骤：步骤1、运动载体的接收机捕获并跟踪卫星信号，解调卫星导航电文，剔除故障卫星和异常卫星；步骤2、利用接收到的前几个历元的卫星信息解算运动载体的初始状态变量X0、方位角S0、角速度Vr0、方差P0；步骤3、确定采用的Kalman滤波方程模型，假设没有系统控制输入，根据不同载体类型和不同的接收机类型确定以下参数：过程激励噪声协方差矩阵为Q，观测噪声协方差矩阵为R，转换矩阵Φ，测量矩阵H，马尔可夫过程在三个坐标轴方向的相关时间常数τx、τy、τz；取状态变量为X＝[x vx αx εx y vy αy εy z vz αz εz]T，观测变量Z＝[Lx Lvx Ly Lvy Lz Lvz]T；其中，分别为X、Y、Z轴上的加速度方差矩阵，分别为X、Y、Z轴上的定位误差方差矩阵；其中(x vx αx)、(y vy αy)、(z vz αz)3组状态变量分别为xyz三个坐标轴上的位置、速度、加速度分量，εx、εy、εz分别为各种误差源在三个坐标轴方向造成的总位置误差，观测变量(Lx Lvx)、(Ly Lvy)、(Lz Lvz)分别为接收机解算出的xyz三个坐标轴上的位置、速度分量；R＝diag[Rx Ry Rz]，其中Rx、Ry、Rz分别为X、Y、Z轴上的观测噪声协方差分量；步骤4、根据现时刻接收卫星信息初步解算位置、速度、载体运行方位角，然后根据前一时刻的位置、速度、加速度，利用卡尔曼滤波方程解算现时刻的位置、速度和加速度；具体包括如下步骤：步骤401、检测接收卫星信息是否良好以解算当前载体运行方位角，若是，利用如下卡尔曼滤波方程解算当前时刻的状态变量返回退出；若否，转到步骤402；X^(k,k-1)=ΦX^k-1---(1)]]>X^k=X^(k,k-1)+Kk(Zk-HX^(k,k-1))---(2)]]>Kk＝P(k，k‑1)HT(HP(k，k‑1)HT+R)‑1P(k，k‑1)＝ΦPk‑1ΦT+QPk＝(I‑KkH)P(k，k‑1)式中，是利用K‑1时刻状态变量预测的K时刻状态变量预测结果，是K‑1时刻的状态变量最优结果，是K时刻的状态变量最优估算值，Kk为卡尔曼增益；Pk为后验估计误差的协方差矩阵，P(k，k‑1)为先验估计误差的协方差矩阵；步骤402、将k‑1时刻状态变量由XYZ坐标系转换为NEU坐标系，设NEU坐标系下的速度变量为Vn0、Ve0、Vh0：步骤403、计算添加角速度辅助后的速度变量：Vn＝(Vn0*Vn0+Ve0*Ve0)1/2*cos(Sk‑1)Ve＝(Vn0*Vn0+Ve0*Ve0)1/2*sin(Sk‑1)Vh＝Vh0步骤404、；令Vrk‑1＝Vrk‑2，Vrk‑1、Vrk‑2分别是K‑1、K‑2时刻的角速度；根据辅助方程角速度：Vrk‑1＝(Sk‑Sk‑1)/T计算Sk＝Sk‑1+Vrk‑1*T，Sk是K时刻的载体运行方位角；步骤405、将NEU坐标系下的速度变量Vn、Ve、Vh转换为XYZ坐标系，设此时XYZ坐标系下的速度变量相应为vx1、vy1、vz1，令步骤406、联合Kalman滤波方程，两者进行等权计算现时刻状态变量预测结果：返回退出。