[发明专利]一种基于坐标变换的三维车载定位导航方法

权利要求书

1.一种基于坐标变换的三维车载定位导航方法，其特征在于包括以下步骤：

步骤1，建立m个非线性运动模型j，模型j在k+1时刻的表达式为是模型j在k时刻时均值为零的白噪声序列，是模型j在k时刻的概率，运动模型之间通过马尔可夫链来控制转移；

步骤2，建立无迹卡尔曼滤波的非线性观测方程Z_k＝h(X_k,V_k)，Z_k是观测向量，h(·)是观测的非线性函数，V_k为系统观测噪声序列，观测向量维数必须小于或等于状态向量的维数；

步骤3，模型i到模型j的转移概率为p_ij，计算在k-1时刻模型i到模型j的混合概率μ_ij[(k-1)/(k-1)]，计算模型j在k-1时刻的混合状态估计和混合协方差估计P_j[(k-1)/(k-1)]，将计算后得到的混合估计作为循环的初始状态；

步骤4，将模型j在k-1时刻的混合状态估计经过U变换计算出sigma点值及其相应的一阶权值和二阶权值其中i表示sigma点的序号；

步骤5，将sigma点值通过模型j的状态方程，计算出状态的一步预测sigma点值用状态的一步预测sigma点值计算出状态的一步预测值和一步预测协方差矩阵其中，上标x表示此矩阵是状态值的协方差矩阵，一步预测值为混合状态估计在k时刻的值；

步骤6，将步骤5中的一步预测sigma点值通过系统观测方程，求出观测值的sigma点值计算观测预测值为Z_j[(k)/(k-1)]、观测协方差矩阵为及状态观测互协方差矩阵为其中上标z表示此矩阵是观测值的协方差矩阵，上标xz表示此矩阵是状态值和观测值的互协方差矩阵；

步骤7，测量更新，计算增益阵K_j，计算模型j在k时刻的状态估计值以及误差协方差矩阵

步骤8，计算更新后的概率模型j的似然函数Λ_j(k)，加入新的观测信息Z(k)后，更新当前模型概率；

步骤9，输出交互，各个模型并行进行以上步骤，更新各自模型概率后计算总的状态估计和总的协方差估计P^X[(k)/(k)]；

步骤10，判断车辆是否处于坡面，若否，则返回步骤3继续执行，反之则建立平面坐标系S和坡面坐标系S1，根据两个坐标系变化前后的关系，构建变化前后车辆三维位置坐标之间的方程式，由此计算出车辆在坡面的高度信息，之后再返回步骤3继续循环执行。

2.根据权利要求1所述的基于坐标变换的三维车载定位导航方法，其特征在于：步骤4中，sigma点值的具体计算方法如下：

式中，n为状态向量维数，P_j[(k-1)/(k-1)]为混合协方差估计，λ为缩放比例参数，其中i表示sigma点的序号。

3.根据权利要求1所述的基于坐标变换的三维车载定位导航方法，其特征在于：步骤5中，计算状态的一步预测值和一步预测协方差矩阵的具体方法如下：

其中，为步骤4中的sigma通过模型j的状态方程后的一步预测sigma点值。

4.根据权利要求1所述的基于坐标变换的三维车载定位导航方法，其特征在于：步骤6中，计算观测预测值为Z_j[(k)/(k-1)]、观测协方差矩阵为及状态观测互协方差矩阵为的具体方法如下：

其中，Q_k-1为观测噪声协方差矩阵，观测值的sigma点值。

5.根据权利要求1所述的基于坐标变换的三维车载定位导航方法，其特征在于：步骤9中，计算总的状态估计和总的协方差估计P^X[(k)/(k)]的具体方法如下：

其中，μ_j(k)为k时刻运动模型j的概率。

6.根据权利要求1所述的基于坐标变换的三维车载定位导航方法，其特征在于：步骤10中，计算车辆三维位置坐标之间方程式的具体方法如下：

其中，为坐标系S变换的方向角，θ为坐标系S变换的仰角，ex1、ey1、ez1是坐标系S1的坐标轴单位向量，ex、ey、ez是坐标系S的坐标轴单位向量。