[发明专利]一种基于误差卡尔曼滤波的高动态车辆姿态估计方法

权利要求书

1.一种基于误差卡尔曼滤波的高动态车辆姿态估计方法，其特征在于：

包括以下步骤：

S1:建立系统状态更新方程：其中，

x为状态向量，δx为误差状态向量；

S2：系统采用东-北-天导航坐标系，其状态预测方程展开为：

p←p+vVt+1/2(R(α_m-α_b)+g)Vt²

v←v+(R(α_m-α_b)+g)Vt

α_b←α_b

ω_b←ω_b；

S3:建立先验估计协方差方程为：

其中

其中

为加速度计白噪声

为陀螺白噪声

σ_αω为加速度计零偏稳定性

其中σ_ωω为陀螺零偏稳定性；

S4：建立汽车运动情况下的系统运动模型，忽略地球自转速度，假设车辆的俯仰加速度、横滚角速度和垂直速度都为0，可建立车辆动力学方程：

从而得到横滚角和俯仰角的观测量；

S5:将S4得到的横滚角和俯仰角进行ESKF建模纠正，

Z_k＝[φ θ]

δZ_k＝H₁δx+η1＝[φ θ]^T-[φ_DR θ_DR]^T

其中:

滤波增益矩阵更新方程为：

误差状态更新方程：

δx＝K_kδZ_k

后验估计协方差更新方程为：

其中I_KH＝I-K_kH₁；

S6：预测GPS的航向信号和位置信号是否可用，为了检测GPS信号是否可用，预测的公式为：

利用实时求取各级模型参数，为下一时刻GPS的位置信息，通过上面的公式就可以实时在线估计出GPS输出定位信息，以达到处理的目的，判断性准则：利用95％的圆周半径作为GPS接收输出定位信息的判断标准，将3级AR模型的预测输出值与当前时刻GPS接收机定位输出值相比较，当两者的差距超过该半径，则认为5％的小概率事件发生了，当前的GPS输出定位信息不可用，判定该时刻的GPS输出值为离群值，95％的圆概率半径的计算式如下：

式中，HDOP为水平精度因子，取为1.5；σ_UERE为卫星伪距测量误差的标准偏差，对于C/A码来说，其估计值为33.3m；

如果当前GPS输出的信息为离群值，则为了不影响下一时刻模型的准确性，应该将输出信息剔除掉，并以当前时刻的AR模型的预测输出值代替，继续进行在线的参数估计和下一时刻的预测，并判断信息的可用性；

S7：当GPS航向信号判断可用后，对航向角观测量进行ESKF建模矫正，航向角的输出值为ψ，

Z_k＝[ψ]

δZ_k＝H₂δx+η2＝[ψ]^T-[ψ_DR]^T

其中：

H₂＝[0_1×3 0_1×3 0 0 I 0_1×3 0_1×3]

η₂＝[ξψ]

滤波增益矩阵更新方程为：

误差状态更新方程为：

δx＝K_kδZ_k

后验估计协方差更新方程为：

其中I_KH＝I-K_kH₂；

S8:当GPS位置信号判断可用时，对GPS位置观测量进行ESKF建模矫正，GPS点从WGS84坐标系转换成BJ54的坐标系，从而GPS位置信息的高斯吕克投影坐标为Z_k，

Z_k＝[P_x P_y P_z]

δZ_k＝H₃δx+η₃＝[P_x P_y P_z]^T-[P_xDR P_yDR P_yDR]^T

其中：

滤波增益矩阵更新方程为：

误差状态更新方程为：

δx＝K_kδZ_k

后验估计协方差更新方程为：

其中I_KH＝I-K_kH₃。