桂林电子科技大学毕业设计（论文） 第16页 共49页
?0,k?j?1,k?j?kj??
下面直接给出随机线性离散系统基本卡尔曼滤波方程如下表所示3.1所示：
表3.1 卡尔曼滤波器的递推公式 系统状态方程 系统观测方程 噪声统计特性 E[WkWj]?Qk?TjkXk??k,k?1Xk??k,k?1Wk?1 Zk?HkXk?Vk E[Wk]?0,E[Vk]?0 ,E[VkVj]?Rk?TTjk,E[WkVj]?0,k?jTTE[X0,Wk]?0,E[X0,Vk]?0 滤波公式 预测公式 滤波增益 滤波误差协方差 预测误差协方差 初始条件 ?Xk??Xk,k?1?Kk[Zk?Hk?X??k,k?1?Xk?1k,k?1] ?Xk,k?1 ?1Kk?Pk,k?1Hk[HkPk,k?1Hk?Rk] Pk?(I?KkHk) TTPk,k?1??k,k?1Pk?1??X0Tk,k?1??k,k?1Qk?1?Tk,k?1 ?u,P0?P0 X0和P0，根据K时刻的量测以上为离散型卡尔曼滤波基本方程，只要给定初值见?Zk，就可以递推计算得到K
Xk(K=1，2，?)。根据上述的滤波公式，时刻的状态估计?增益矩阵kk与初始均方误差阵P0、动态噪声协方差阵Qk及观测噪声协方差阵之间的存在着下列关系:
①当P0、Qk 、Rk(K=1，2，3…)同时乘以相同数量时，kk不变。
②当Rk增大时，kk变小，这从直观上可以理解，因为如果观测噪声增大，则增益应该取的小一些，以减弱观测噪声的影响。
③当P0变小或Qk变小或两者都变小时，kk变小。这从直观上看也是自然的，因为
P0变小表示初始估计较好，Qk变小表示状态转移的随机波动小，所以新加进的
观测对状态预测的校正影响减弱，于是增益矩阵kk应当变小。
从上面的卡尔曼滤波器方程可以得到卡尔曼滤波器结构框图如下图3.1所示，该滤波器的输入是系统状态的观测值，输出的是系统状态的估计值。
桂林电子科技大学毕业设计（论文） 第17页 共49页
图3.1 卡尔曼滤波器结构图
离散型卡尔曼滤波算法如下图3.2所示：
图3.2 卡尔曼滤波的两个计算回路和两个更新回路
从图中可明显看出卡尔曼滤波具有两个计算回路:增益计算回路和滤波计算回路，其中增益计算回路是独立回路，而滤波计算回路依赖于增益计算回路。
在一个滤波周期内，从卡尔曼滤波在使用系统信息和量测信息的先后次序来看，卡尔曼滤波具有两个明显的信息更新过程:时间更新过程和量测更新过程。状态一步预测方程说明根据K-1时刻的状态估计预测K时刻状态估计的方法，一步预测误差方差对这种预测的质量优劣作了定量描述。着两式的计算中仅使用了与系统动态特性有关的信息，如一步转移矩阵、噪声输入矩阵、过程噪声方差矩阵等。从时间的推移过程来看，
桂林电子科技大学毕业设计（论文） 第18页 共49页
这两式将时间从K-1时刻推进到K时刻，描述了卡尔曼滤波的时间更新过程。卡尔曼滤波方程的其余诸式用来计算对时间更新值的修正量，该修正量由时间更新的质量优劣(Pk/k?1)、量测信息的质量优劣(Rk)、量测与状态的关系(Hk)以及K时刻具体的量测值Zk所确定，所有这些方程围绕一个目的，即正确合理地利用量测Zk，这一过程描述了卡尔曼滤波的量测更新过程。 3.5 交互式多模型（IMM）算法
模型是IMM算法的基础，在IMM算法中使用比较广泛的有基于匀速直线（英文缩写为CV）模型、基于当前统计英文缩写（CS）模型、Singer模型和基于匀加速（英文缩写CA）模型。载体的运动是复杂多变的，任何单一模型都无法全面地描述，但使用过多模型不仅会增加计算量，还会使算法性能下降。基于当前统计（CS）模型的算法能较好的跟踪机动载体运动，但是在载体做匀速运动时滤波效果则明显不如基于匀速直线（CV）模型算法。而交互式多模型（IMM）算法引入到载体导航定位中，能针对目标不同的运动状态确定不同模型的子滤波器，然后依据模型概率对子滤波器状态加权求和，作为最终的估计结果。下面主要介绍IMM自适应滤波算法的具体过程。 3.5.1机动载体目标模型
在应用估计理论特别是Kalman滤波理论时，要求定义估计问题的数学模型来描述某个时刻状态变量与以前时刻的关系。一般来说，状态变量的增加会使估计的计算量相应增加，因此在满足模型的精度和跟踪性能的条件下，常采用简单的数学模型。
假定GPS数据接收周期为T，可获得车辆的位置数据，平面上的车辆机动目标按照恒速直线运动的轨迹运动，其转弯、机动等引起的加速度则看作是恒速直线运动的摄动。那么，在直角坐标系中，该目标运动的数学模型可用下列差分方：
?(k?1)?X?(k)?Ta(k) （3.16） X x?(k)? X(k?1)?X(k)?TXT12ax(k)T2 （3.17）
T?(k)??x?(k),y?(k)?分别表示GPS接收机第K次发送信上式中，X(k)??x(k),y(k)?和X号时车辆在X,Y方向上的位置和速度。不妨设车辆的加速度ax(k)是平稳随机序列，服从零均值、方差为?a的正态估计，且在某一时刻加速度ax(k)和另一时刻的加速度ax(l)不相关，即E?ax(k)??0，E?ax(k)at(l)???a2?I，其中I为2×2阶的单位矩阵。 1.恒速(CV)模型(非机动模型)
假定GPS数据接收周期为T，当车辆以恒定速度运动时，状态变量为X??e,ve,n,vn?，
T2e、n分别为东向及北向的位置，ve、vn分别为东向和北向速度，则状态方程和观测方程为：
桂林电子科技大学毕业设计（论文） 第19页 共49页
?X(k?1)??X(k)?GW(k)?Z(k)?HX(k)?V(k) ? （3.18）
式中，
?1?0????0??0T1000010?T0??2?0?，G??1?0T???0???02??0?T?2?1???100010???，W??w10?，H???0w2?T
系统噪声W为零均值、方差阵为Q的高斯随机序列；观测噪声V为零均值、协方差阵为R的白噪声，与W互不相关。W、V在两坐标方向上的加速度相互独立并具有相同的方差?a，即：
Q??a?I,E?W(k)??0,E[W(k)W(j)]?Q?kj。
2T22.恒加速(CA)模型(机动模型)
假定GPS数据接收周期为T，当车辆为恒定的加速度在运动，系统的状态变量为
Xm??e,ve,ae,n,vn,an?，e、n分别为车辆东向及北向的位置，ve、vn分别为车辆东向
T及北向速度，ae、an分别为车辆东向及北向加速度。
状态方程和观测方程可表示为：
Xm((k?1)??Xmmm(k)?GWmmm(k)Z(k)?HXm(k)?V(k) （3.19）
式中，
?1??0?0???0??0??0mT10000mT000100000T10T22T1000?m?T0?4??2?T0?2??0m?1?G?，2?0T?2??T??0?1???020??0??0?H，2?T4?T?2?1?m?1???0000100000??0?，
Wm?w1w2??，
其中系统噪声Wm及观测噪声Vm与恒速模型相同。 3.5.2IMM算法
IMM算法是只考虑状态量测时的滤波算法,输出没有延迟。算法基本分为四个步骤:输入交互、各子滤波器滤波计算、模型(模式)概率更新、输出交互。为克服精确滤波所需的模型假设数目随着时间呈指数倍增长,它在输入交互时,用一个高斯分布来近似多个高斯分布之和。在模型概率计算时,根据状态量测信息来调整更新模型概率,以适应实际
