一种基于坐标变换的三维车载定位导航方法

摘要

本发明公开了一种基于坐标变换的三维车载定位导航方法，该方法首先建立多个非线性运动模型；接着对车辆的各个运动模型概率进行混合估计，各个运动模型分别计算自己的滤波初始值；然后融合无迹卡尔曼滤波，加入GPS观测值，并行计算各模型的滤波输出，在滤波结束后更新各模型的概率，找到最符合当前车辆运动的模型进行输出交互，得到总的状态估计和总的协方差估计；最后当车辆是在坡面状态时，利用坐标变换的方法建立平面和坡面两个不同的坐标系，得出车辆位置三维坐标变化前后的关系，算出其高度信息。本发明确实有效地提高了车辆在高度方向上的定位精度以及整体的定位精度，具有较强的实用价值。