卡尔曼滤波_b.docVIP

3.2 卡尔曼滤波[卫星导航测量差分自校准融合技术] 当运动目标能用精确动力学（弹道）方程描述其轨迹时，卡尔曼滤波往往是很有效的最优线性滤波器。为此，本章将论述卡尔曼滤波的基本原理以及改进形式。 3.2.1 连续型和离散型的状态和观测模型 假设是需要求解的状态向量，且属随时间变化的随机过程，例如，飞行器的空间飞行轨迹和速度组成的飞行器运动状态向量。下面根据观测数据序列，结合状态的动力学模型来确定状态向量的估计，并给出相应的估计精度。 首先给出状态向量的线性动力学模型 (3-75) 式中-----维状态向量； -----维控制项，它是确定的非随机向量； 、----维和维的已知矩阵； ----维随机噪声。 观测数据的线性观测模型可以表示为 (3-76) 式中 ----维状态向量； ----已知的维矩阵； ----维量测噪声向量。 由线性微分方程解可知，当初始条件为时，微分方程的解可以表示为 (3-77) (3-78) 式中，称为状态转移矩阵，它是齐次方程在初始条件下的解。 在实际测量中，由于观测量通常是离散采样值，需将上述连续型的状态和观测模型转化成离散型。观测模型式（3-76）的离散形式为 (3-79) 式中，是采样时刻。 现将状态方程（3-75）转变为离散模型。记当很小时，认为在上保持常值，于是由方程（3-78）可得 (3-80) 式中，。 为了书写方便，用下标“”表示时刻“”，于是方程（3-79）和（3-80）又可以写成 (3-81) (3-82) 现在已获得时刻和之前时刻的观测数据，由此对状态进行预测、滤波和平滑。在此，主要讨论状态的滤波，即对时刻的状态进行估计。 3.2.2 卡尔曼滤波器原理 假设动力学模型是（3-81）的噪声序列满足 (3-83a) (3-83b) 式中 函数；表示是非负定阵。 模型式（3-82）的测量噪声序列满足 (3-84a) (3-84b) 另外和还满足 (3-85) 如果给定时刻状态的初始估计，且满足 (3-86a) (3-86b) 现在要求在满足上述条件下，由观测集确定状态的最小方差（MV）估计，并记为。 由估计理论可知，状态向量的最小方差估计为条件期望，即 (3-87) 为了推导卡尔曼滤波器的具体表达式，需要引用一些符号。令为由观测集得到的状态的最小方差估计，即由给出的预测值，而预测值的误差协方差阵为，即 (3-88) 如果假设为正态向量序列，可以证明最小方差估计（也就是条件期望）是观测量的线性函数；否则，最小方差估计不是的线性估计。在此，按正态假设条件讨论，于是有 (3-89) 常称为新息序列。 根据上述假设，则预测值估计为 (3-90) 而对应的误差协方差阵为 (3-91) 式中，并且利用了正交投影原理的结果。式（3-89）中根据最小方差估计的要求来确定，也就是说，的选取要使下式成立 (3-92) 由于 (3-93) 式中和。 注意到，于是 (3-94) 而 (3-95) 这里，应用了的结论。 将式（3-95）展开，得到 (3-96) 根据上式，取矩阵的迹，并对求偏导数，即得 (3-97) 再令该式等于零，则可解得 (3-98) 通常称为步增益矩阵。 现在再将式（3-98）变成 (3-99) 该式两边同时乘，并且代入式（3-92），经整理后则得 (3-100) 由式（3-89）~（3-91）、式（3-98）和式（3-100）组成的完整滤波公式，通常称为卡尔曼——布西滤波公式。在给定起始状态的条件下，可用该公式进行递推运算，给出不同时刻的状态最小方差估计。值得注意，上述卡尔曼滤波公式是假设为正态随机变量序列，按最小方差估计要求得到的公式。如正态假设假设不成立，可以按最优线性无偏估计要求，使用其它方法导出类似公式。 为了便于实际应用和理论分析，利用矩阵求逆反演公式来推导另一种形式的卡尔曼滤波公式。当和是正定矩阵时，若有 (3-101) 则 (3-102) 将式（3-98）代入式（3-100），则有 (3-103) 令，，，，则由矩阵求逆反演公式（3-101）和式（3-102），得到 (3-104) 而增益矩阵的表达式为： (