关注微博
组合导航系统算法
(1)组合导航的状态方程
目前，GPS/SINS组合导航方式主要分为松散组合和紧密组合。而松散组合大多采用位置、速度组合模式，而本文中加入双天线GPS姿态角作为量测量进而实现全组合方式。其状态方程如公式(1)所示：
其中F(t)是18*18阶的状态转移矩阵;W(t)为18维的状态噪声向量;下列各式给出了状态方程的系统噪声性质：
系统状态X(t)选取为：
其中δL、δλ、δH为经度、纬度、高度误差，δVe、δVn、δVu分别为
东北天三个方向上的速度误差，Φe、Φn、Φu为平台误差角，εbx、εby、εbz，εrx、εry、εrz，Δax、Δay、Δaz分别为陀螺常值漂移，相关漂移与加速度计的零偏。
而其中失准角误差方程为：
速度误差方程为：
位置误差方程为：
(2)组合导航系统的量测方程
由于系统将定向GPS给出的数据作为修正数据，系统量测值在位置和速度值的基础上加入了姿态信息。位置量测值为惯导系统与GPS给出的纬度、经度和高度差，相应的速度量测值为惯导系统与GPS给出的在惯性系统中各坐标下的差值，而惯导系统与双天线GPS给出的姿态差值作为第三组量测值。
惯导系统与GPS位置量测信息在地理坐标系以真值和误差表示如下：
式中L、λ、h表示运载体真实位置，Ne、Nn、NH为GPS东北天方向位置误差。
位置量测方程如下：
与位置量测方程形式相似，速度量测方程表示如下：
下式为姿态量测方程：
将（2）（3）（4）式组合后可以得到整个系统的量测方程如下
本文采用卡尔曼滤波算法对GPS，IMU数据进行融合。将式(1)(8)离散化后方程为：
一步预测状态方程为：
状态估计方程为：
滤波增益为：
计算得到一步预测均方差：
则均方差误差估计：
-- Use of this website is subject to its terms of use.