式中,状态矢量X和白噪声随机误差矢量W为
而误差系数矩阵 和状态系数矩阵 分别为
式中, 为对应9个基本导航参数的系统阵; 和 分别为
2.2.2. 组合导航数学模型——量测方程
在位置、速度组合方式中,有两组观测量:一组为位置观测量,即惯性导航系统给出的经度、纬度和高度信息和GPS接收机给出的相应信息的差值;另一组为两个系统给出的速度的差值[10]。
惯性导航系统的位置信息为(2.58)
GPS接收机给出的位置信息为 (2.59)
式中, , , 是位置(经度、纬度、高度)的真值; , , 是GPS接收机沿东、北、天方向的位置误差,单位为米,对 和 进行量纲的转换以与惯性导航系统经度、纬度的量纲配合。
位置量测方程为
惯性导航系统的速度信息为(2.63)
GPS接收机给出的速度信息为(2.64)
式中, , , 是载体沿地理坐标系各轴向的速度真值; , , 为GPS接收机测速误差。
速度量测方程为
把位置和速度信息结合在一起,得
(2.68)
式(2.60)、式(2.65)、式(2.68)构成了三组量测方程,分别为位置量测方程、速度量测方程和位置/速度量测方程。各量测方程的文数不同,与系统的状态方程(2.51)配合,可根据实际情况来设计和选用。本设计采用的是速度量测方程。
2.2.3. 卡尔曼滤波基本理论与应用
为获取系统各个物理量的实际数据,需利用测量手段对系统的可测状态进行量测。量测值可能仅是系统的部分状态或部分状态的线性组合,且量测值中含有随机误差(称为量测噪声)。卡尔曼滤波将仅与部分状态有关的量测值进行处理,得出从某种统计意义上估计误差最小的更多(或全部)状态的估值。
卡尔曼滤波是一种线性最小方差估计方法,不仅要考虑信号与观测值的基本统计特性,还要用到系统的状态空间描述。主要采用离散递推的表达形式,即在以前时刻估值的基础上,根据t时刻的量测值递推得到t时刻的状态估值,由于以前时刻的估值是由该时刻以前的量测值得到的,所以这种算法得到的估值是综合利用t时刻及以前所有量测信息[10]。
1. 线性离散卡尔曼滤波方程
常用离散卡尔曼滤波方程作为离散化模型来描述系统,即用离散化后的差分方程描述。设离散化后的系统状态方程和量测方程分别为
式中, 为k时刻的n文状态向量,也是被估计向量; 为k时刻的m文量测向量; 为k-1到k时刻的系统一步转移矩阵( 阶); 为k-1时刻的系统噪声(r文); 为系统噪声矩阵( 阶),它表征由k-1到k时刻各个系统噪声分别影响k时刻各个状态的程度; 为k时刻的量测矩阵( 阶); 为k时刻的m文量测噪声。
满足误差方差最小准则的离散卡尔曼滤波方程为
状态一步预测方程:(2.71)一步预测均方误差方程: (2.72)
滤波增益方程(2.73)
状态估值方程:
(2.74)
估计均方误差方程:
其中,式(2.71)和式(2.72)称为时间修正方程,其他三个为量测修正方程。
上述各滤波方程的物理意义分别为: MEMS惯性GPS信息融合技术的个人导航定位方法(9):http://www.youerw.com/tongxin/lunwen_4213.html