卡尔曼滤波 The Kalman Filtering

本章内容 (Contents in this chapter) 概述 几种统计方法 线性最小方差估计 离散卡尔曼滤波问题 有色噪声的处理 系统的可控性和可观性 非线性模型的处理 发散问题 自适应滤波 卡尔曼滤波应用实例

概述 （Summarization） 二十世纪60年代初卡尔曼提出实现线性滤波的一种新方法——卡尔曼滤波. 卡尔曼滤波处理的信号可以是平稳的，也可以是非平稳的，滤波器可以是定常的，也可能是时变的。

Rudolf Emil Kalman 匈牙利数学家 BS&MS at MIT PhD at Columbia 1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）

60年代以来，组合系统一般都采用卡尔曼滤波技术，即在两个（或两个以上）导航系统输出的基础上，利用卡尔曼滤波去估计系统的各种误差。 在导航系统均为动态系统情况下，系统在不同时刻有观测量输出，系统也属于带随机干扰的动力学系统，状态为随机状态，其状态参量可以在统计意义下求出最优估值. 卡尔曼滤波为递推线性最小方差估计，非常适应于动态导航系统及组合导航系统的状态估计。 下面，我们首先简介常用的几种估计方法，最后推导卡尔曼滤波公式。

几种统计方法 （Few statistic methods） 最小二乘估计 (Least Square estimation) 极大验后估计与极大似然估计 (maximum posteriori estimation ) 最小方差估计 (Minimum Variance Estimation) 线性最小方差估计 (Linear Minimum Variance Estimation)

1、最小二乘估计 (Least Square estimation) 观测向量Z与被估计向量x之间存在以下线性关系: 最小二乘是在 下求得的最佳估值

(maximum posteriori estimation ) 2、极大验后估计与极大似然估计 (maximum posteriori estimation ) 极大验后估计： 在已知实验结果Z=z的条件下被估计量x的条件概率密度为 f(x/z) ,如果 f(x/z)达到极大值，就是相应随机向量的最大可能值 。 把验后概率达到极大作为一种最优准则，并把验后概率达到极大的 ， 称之为极大验后估计。 其验后方程为：

使条件概率密度函数取极大的那个值 ， 满足下列方程 极大似然准则 使条件概率密度函数取极大的那个值 ， 满足下列方程 为极大似然估计

设观测值 的观测方程为： 则： 极大验后估计： 方差： 极大验后估计考虑了参数X的先验统计特性，因此，当参数的先验期望x与先验方差Dx已知时，极大验后估计改善了最小二乘估计，因为极大验后估计的方差小于最小二乘的误差方差

(Minimum Variance Estimation) 3、最小方差估计 (Minimum Variance Estimation) 定义：设 是被估计随机变量，Z是 的测量矢量， 如果估计量 ，对一切估计量 ，有 则称 为 的最小方差估计量。 当 时

定理 的最小方差估计 的条件数学期望即 是在测量矢量为Z的条件下， 当X和L都是正态随机向量时，X的最小方差估计 和它的极大 验后估值 存在如下关系： 否则，二者不相等。

4、线性最小方差估计 (Linear Minimum Variance Estimation) 极大似然估计，极大验后估计，都要求知道被估计量 与测量值Z的概率分布，如 或 等。 为了放松对概率分布的要求，就需要对估计量的函数类型加以限制，即把估计量限制为测量值的线性函数，在线性估计量范围内，寻求估计量的最小方差估计，即线性最小方差估计。

一、定义 是被估计量，Z是 的测量值，如果 则称 为 在Z上的线性最小方差估计，记作 或 线性函数集合中，对 设 是被估计量，Z是 的测量值，如果 则称 为 在Z上的线性最小方差估计，记作 或 所谓X在Z上的线性最小方差估计，指的是在测量矢量Z的所有 线性函数集合中，对 具有最小均方误差的一个线性函数。

二、定理 在Z上的线性最小方差估计 ，等于 在Z上 的正交投影，即 为满足正交条件， 需满足： 该条件为 线性估计量最小方差估计的充分必要条件

三、线性最小方差估计的表达式 把最小估计，加上线性（测量值Z的线性函数）的限制，目的在于放松对于概率分布的要求，下面的定理给我们指出，根据被估计量 与测量Z的一、二阶矩，经过简单的计算，就可以确定出 的线性最小方差估计 ，不需要了解 和Z的全面概率分布。 定理 ： 在Z上的线性最小方差估计为： 由上可知，当（x，z）服从联合高斯分布时，线性估计中的最小方差估计，也是一切估计中的最小方差估计

四、线性最小方差估计的性质 （1） 所以， （2）

（3） 即估计误差向量与观测向量L时不相关的，也就是二者正交 （4） 当X，L的联合概率密度为正态时，因为 所以，X的先行最小方差估计量 就等于最小方差估计量

离散卡尔曼滤波 Discrete Kalman Filter 一、被估计信息模型—状态差分方程(State Equation) 是被估计的具有随机扰动的线性动态系统的状 态矢量序列，假定它的动态方程或状态差分方程为： 其中， 为 维状态矢量（被估计量） 为 阶的一步状态转移阵，具有如下特点

为 维动态（或系统）噪声矢量 为零均值的白噪声矢量序列，即 ——已知非负定阵 ——克罗尼克 函数 —— 阶动态噪声驱动阵。

根据状态方程： 由此类推，对于任意的正整数k

上式即为系统在 时刻的状态矢量 t0与直到tk-1时刻的动态噪声矢量 、初始状态 之间的线性关系式。 由于初始状态 不受噪声矢量 的影响， 与 不相关，且 具有零均值, 即 因为 （ 与 正交） 所以

二、观测方程 （Observation Equation） 需估计 ，必须先对 进行观测，或者是 对Xk的某种变换进行观察，这种观测带有一定的观测误差。 观测矢量Zk与状态矢量Xk以及观测vk具有如下线性关系 为tk时刻m维观测矢量 为m×n阶的观测阵 ，

—— 维观测噪声矢量， 为零均值的白噪声序列。 i) ii) ——是已知的正定阵 一般情况下，动态噪声序列{wk}与观测噪声序列{Vk}之间没有直接联系，又由于X0不受{wk}的影响，因此可以假定X0 、wk(k=0,1,2…)与Vl(l=0,1,2…)为互不相关的随机矢量。

三、 的递推方程 1、Kalman滤波需要解决的问题 规定两个常用符号 (l 为时刻） ( 为1到l时刻的所有观测量） ——为 三、 的递推方程 1、Kalman滤波需要解决的问题 规定两个常用符号 (l 为时刻） ( 为1到l时刻的所有观测量） ——为 上的线性最小方差估计（或 在 上的正交投影） 当 为 的滤波值 为 的预测值 为 的平滑值

卡尔曼滤波所需解决的问题，根据 （1） tk-1时刻状态矢量Xk-1的线性最小方差滤波值 （即 在 上的正交投影） 和（2）tk时刻的新观测值Zk, 求出 时刻状态矢量 的线性最小方差滤波值 上的正交投影）的递推方程。

2、 的递推方程 根据 求 的线性递推方程， 在 上 及 分两步解算这个问题，其确定出 的线性最小方差估计 ，然后再根据 及新观测值 确定出 上的线性最小方差 估计

假设已经得到Xk在t1， t2， t3… tk-1时刻的观测值Z1， Z2， Z3… Zk-1 ，但tk时刻的观测Zk未知，则有: （1） 在 的线性最小方差估计 假设已经得到Xk在t1， t2， t3… tk-1时刻的观测值Z1， Z2， Z3… Zk-1 ，但tk时刻的观测Zk未知，则有: 状态方程 观测方程 根据 得 则 在 的线性最小方差估计为：

则有： 该式为卡尔曼递推滤波的第二个方程，这个方程 指出，在线性最小方差估计的意义下， 的滤波值 是由一步预测值 ，利用第 新提供的信息 次观测值 确定的。 是前 次观测值 已提供的关于 的信息， 则是第 次观测的新信息，因此 通常称为第k次观测的新息。

（3）求增益矩阵

（4）求 （5）求

四、滤波公式总结，框图及几点说明 1、滤波公式 a. 初始值，先验统计特性 已知 b. 动态方程和观测方程

c.预报 d.计算增益矩阵 或

e.滤波估值 或

3、滤波方程的几点说明 （1）滤波初值 的选定 要求无偏，即 ， ，才能保证滤波值 对于 具有无偏性质，但在一般情况下， 与 是不容易精确估计到的，当滤波值 与 选取与 ， 不一致时，若动态方程及观测方程为一致完全可控制 和一致完全可观测时，滤波值 与均方误差阵 将渐近的 与初值 ， 的选取无关，这时滤波值 与 的误差 也会逐渐消失。

大表示初始估计的随机误差大， 2、 与 、 的关系 从增益矩阵计算可看出 增大， 变小，因为 大表示 观测噪声大，观测值不可靠，于是增益K，应取小一些， 以减弱观测噪声的影响， ， 增大， 增大, 大表示初始估计的随机误差大， 大表示伴随第K步状态转移的随机波动大， 这时需要加强新观测值对状态预测值的修正，于是Kk应取得大一点。

五、状态方程含有已知控制项时的递推滤波方程组 设系统的状态方程 为已知的控制项 测量方程 这时 在 上的线性最小方差估计

例：假设用雷达观测飞行目标的距离l，目标沿距离方向的加速度为a为 常数，于是目标沿距离方向的运动方程是： 令距离 ， ，

令 ， 雷达直接观测斜距 观测方程

有色噪声的处理 上一节讨论的卡尔曼滤波的基本方程，假定动态噪声 与测量噪声 都是零均质的白噪声序列。 由于实际物理模型所得出的状态方程与测量方程的 附加噪声常常都是有色的（即与时间相关的），这时， 为利用卡尔曼滤波基本方程估计各观测时刻的状态， 就需要对噪声进行处理。 下面介绍有色动态噪声和有色测量噪声。

一、随机序列的成形滤波器 设 为零均值的随机矢量序列，其相关函数为 ，是一不为零的正定阵。 可以通过差分方程 （A）描述， 其中 为与 不相关的白噪声序列， 上式线性系统即为随机序列{NK}的成形滤波器

应用上述线性系统存在如下两个问题需要解决 1、在什么条件下，随机序列 才能通过成形滤波器来描述 可以证明，如{Nk}的相关函数阵Rk,j 满足关系式 则{Nk}必满足（A）式，这时我们称{Nk}为宽马尔可夫序列，即可以用成形滤波器来描述。

及白噪声序列 2、在（A）式中的转移阵 的方差阵如何确定 方差阵

二、有色动态噪声{Nk}的处理 设系统的状态方程为 统计特性如下 (i) ——零均值的宽马尔可夫噪声序列 其中

（ii）{vk}——零均值的白噪声序列 1、应用扩大状态矢量法，将状态方程转换为标准的卡尔曼 滤波的标准形式 写成矢量一矩阵的形式 令 待驱动项为噪声的扩大状态方程如下

2、观测方程 转换后标准的卡尔曼滤波为：

例：惯导初始对准，如果忽略对准时两个水平回路之间的交联关系，对东向加速度计和北向陀螺组成的回路来讲，平台误差角的微分方程为 是北向陀螺漂移，如果 是有色噪声，它包括一阶 马尔柯夫过程 ，随机常数 （逐次漂移）和白噪声 ，则 和 可由以下微分方程描述

将 和 作为系统的状态，则系统状态方程扩充为 这时噪声为白噪声矢量，符合卡尔曼滤波的需求。

三、有色测量噪声的处理 设系统的状态方程为 其中 为零均值的白噪声 为零均值的宽马尔可夫序列，其协方差阵: 其成形滤波器的差分方程为:

为零均值的白噪声序列，根据 采用扩大状态矢量法来处理 得扩大状态方程如下

相应地 的测量方程也可以写成 的测量方程 其中

应用卡尔曼滤波的基本方程即可得到 其中 就是 的滤波值 由上述扩大状态矢量法处理有色测量噪声过程可以发现： 的测量方程缺噪声项，因而其相应的增益阵为： 上式中，等式右边的逆阵未必存在，为回避这一风险， 通常情况下可采用等效测量方程来处理。

系统的可控性和可观性 一、卡尔曼滤波的稳定性问题 卡尔曼滤波要求 这样,卡尔曼滤波才具有无偏和估计均方误差最小的特性，实用中 的一、二阶统计特性 和 往往不能准确得到。

在这种情况下，滤波必须满足稳定性要求，否则，不同的初 始条件会导致滤波器得出不同的估值，满足滤波稳定性是指： 1、随着时间的增长，估值 逐渐不受初始值 的影响 2、随着滤波时间的增长， 逐渐不受初始估计均方误差 估值和估值方差也会逐渐趋于相同 的影响,这样，即使初始条件不同，随着滤波时间的增长

二、满足滤波稳定性的条件 卡尔曼首先提出随机可控性和随机可观测性作为判别滤波 稳定性的条件。以离散时间系统为例： 条件： 如系统为一致完全随机可控和一致完全随机可观，则该系统噪声强度阵Q和观测噪声方差强度阵R都是正交，这时，卡尔曼滤波器具有一致渐近稳定性

随机可控和可控的意义 可控：系统确定性输入（控制）影响系统状态的能力。 随机可控：系统的随机噪声影响系统的能力。 如系统一致完全可控，只要有一段时间间隔，系统噪声就能对所有的状态产生影响。 随机可控与滤波稳定性是密切相关的，如系统一致完全可测，观测值即使只能测量部分状态或部分状态的线性组合，但因系统各状态间相互联系和转移关系，只要有一段测量时间，再加上随机可控的前提，总能从带有噪声的观测值中估计出所有的状态。 随机可控与随机可测是判别滤波稳定性的充分条件，但不是充要条件

三、滤波稳定性判别方法 1、定常线性系统卡尔曼滤波的稳定性 为常数阵 定常线性系统的一致完全可控与一致完全可测的充要条件 n为状态的维数

正定说明估计开始时刻，所有状态的估计误差均方差都不为零， 2、判断的方法 为正定，推广形式的随机可控阵为： 为正定， 非奇异，满足可控。 的基础， 是 正定说明估计开始时刻，所有状态的估计误差均方差都不为零， 所以 同样提供了估计的前提。 系统各状态在估计开始时刻一般 都不可能有准确无误的估值，所以 正定的条件较易满足， 从这个意义上讲，随机可观是滤波器是否稳定的主要条件。

①可采用检验随机可控阵Cs和随机可观测阵Os来判别滤波稳定性是充分条件。 如条件不满足，滤波器也可能是稳定的，也可能是不稳定的，但滤波器仍具有实际的滤波效果。 ②采用随机可观测阵和推广形式的随机可控阵来判别滤波稳定性。 ③也可仅检验系统可观测性，因其它主要条件如果不 满足，可从物理意义来分析其影响，从而为滤波器设计出合理的方案。

惯性/多普勒综合导航系统可观测性分析 利用多普勒雷达测出飞机地速与惯导系统进行速度综合。 设惯性仪表的误差为白噪声，则东向回路和北向回路可以解耦，东向回路的状态方程，简化为： 动态方程 ——惯导东向速度 ——平台北向误差角 ——东向加速度误差 ——经度误差 ——陀螺漂移

观测方程 利用惯导东向速度 与多普勒测出的地速 的差值作为综合滤波器 的量测值，并设 的误差为白噪声 ，方差为R 设

系统不可观测 观测 可观测出平台误差角 ， 都不受 的影响，无法从连续观测值中观测出 ，所以 为不可测状态，如果连续系统 说明， 受 的影响而产生 ，说明两个状态估计误差之间 的 因 所以从物理意义上讲，由于 有交联关系， 而下降， 的初值无法观测出来， 的被估计作用，使 的增长受到抑制 ，虽然 但这样的组合同样使位置误差 有一定的估计作用，所以，虽然系统 不满足可观测条件，滤波器在一定程度上仍有效。

上述分析表明： （1）利用可观测性来初步判断滤波器的稳定性较为简单。 （2）系统不满足可观测性，不应就此得出滤波无效的结论，可进一 步分析不可观测的原因，以确实滤波是否有效。 （3）不可观测原因是由于存在不可分辨状态。系统是只能观测某些 状态的线性组合 ， 为必须估计状态， 为其它状态， 的值不大，则可略去 ， 变化为 ，估值 仍然有效。 （4）如不可观测原因是单独存在不可观测状态，但是主要受其他 状态的影响而不影响其它状态，则将它作为被估计状态之一， 滤波器对它的估计仍有一定的作用。

非线性模型的处理 标准卡尔曼滤波方程，除了要求动态噪声与测量噪声是零均值的、互不相关的白噪声序列之外，还要求系统的状态方程与测量方程都是线性的，但在工程实践中，系统的物理模型有时需要通过非线性方程来描述，这时要利用标准卡尔曼滤波方法估计系统的状态，必须对非线性模型线性化。

发散问题 在滤波计算中，常常会出现这样的现象，当观测值的数目不断增大时，按模型计算的滤波均方误差趋于零或某一稳定值，但滤波的实际误差（滤波值对实际状态的偏差）却趋于无穷大或远远超过滤波按模型计算的均方误差允许的范围，使得滤波值失去作用，这种现象称为滤波发散

一、发散现象的原因 系统数学模型与噪声的统计模型不准确，不能反映出真实物理过程，使得模型与真实的观测值不匹配，这样就可能引起发散，这种发散叫滤波发散。 由于计算过程中，舍入误差的积累， 逐渐失去非负定性或对称性

例，设飞机的真实飞行高度 以等速度 不断增加，即真实的 高度化方程为 ，则 取 秒， 状态方程 为： 高度的实际测量方程为 若设计滤波器时把高度的模型错误地取为 测量方程仍给为 取 滤波方程为：

（1） （2） （3） （4） （5） = 从(3)(4)看出，当 及增益 都趋于零即

由上述过程可以给出如下结论： 当滤波模型不准确时，易造成滤波发散。可以通过加大当前观测值权，相应地减小老观测值权来扼制滤波发散现象的出现。衰减记忆滤波和限定记忆滤波，都是在这个思想的基础上给出的。 在滤波值中新观测值的作用太小，老观测值的作用太大，常引起滤波发散。因此，逐渐减小老观测值的权，相应地增大新观测值权，是扼制滤波发散的另一个可行途径。衰减记忆滤波就是这种扼制滤波发散的次佳线性滤波方法。

自适应滤波 自适应滤波也是一种具有抑制滤波器发散作用的滤波方法，它在滤波计算中，一方面利用量测不断地修正预测值，同时也对未知的或不确切知道的系统模模型和噪声统计参数进行估计或修正。自适应滤波的类型很多，大体上可分为贝叶斯法、极大似然法、相关法与协方差匹配张四类。本节只介绍系统模型参数已知，而噪声统计参数Q和R未知或不确切知道时的自适应滤波。 对于一些系统状态方程和观测方程都精确已知 ,但动态噪声和观测噪声的统计特性却是未知的 ,为此 ,可采用 Sage自适应滤波 (又称为极大后验估计器 )。根据每次测出的更新信息 ,推算出动态噪声和观测噪声的统计特性 ,并使滤波器成为最优 ,这就是 Sage自适应滤波的思路。

对于状态方程和观测方程 ： Xk=Φk,k-1Xk-1+ωk – 1 Zk=h(Xk,k)+Vk 设 Φk, k- 1 h(XK)已知 ,ωK - 1 ,VK 为独立的正态白噪声 ,其统计特性为：E(ωk)= q　E(Vk)=r 噪声的均值 q,r及协方差 Q,R是未知的 ,Sage自适应滤波就是基于观测值 (Z1 ,Z2 ,… ,Zk)求得状态估值 Xk,并由极大验后估计原理 ,可得极大验后估值  分别为 :

初始条件为 q0 ,Q0 ,R0 ,r0 ，E(X0 )=X0 , 0 ,var(X0 )=D0 , 0 ，用滤波估值 和预极值 ，近似代替计算复杂的平滑估值，可得次优极大验后估值为 为观测残差。 式中 为预测残差， 可以证明,上述次优估计具有无偏性。可见 ,Sage自适应滤波是当 q,r,Q,R未知时 ,利用预测残差和观测残差求每一时刻估计动态噪声 和观测噪声的均值及其协方差 。

卡尔曼滤波应用实例 一、坐标推算系统和GPS定位组合数学模型 1、坐标推算系统数学模型 坐标推算系统测定航向和航速，航速可由惯性系统，多普勒声纳， 电磁计程仪测定，航向可由平台罗经，电罗经，磁罗经，测向仪提供。 ， 初始纬度，经度 ——航向 V——航速 ——计算出纬度，经度的改正量 ——定位时间间隔 ——地球半径

2、组合导航数据处理 坐标推算系统测定了船的速度V和航向a, GPS观测值为伪距，船的状态 随时间变化，并且具有一定的规律。 （1）动态方程 设状态变量 分别为纬度，经度 ——航速 ——伪距 ——伪距误差 ——航向 ——子午圈曲率半径 ——平行圈曲率半径 对 求偏导，

因观测方程是非线性的，在卡尔曼滤波中求状态的改正数 则上述参数的矩阵表达式为：

（2）观测方程 GPS观测值差为伪距，对于第i颗卫星的伪距观测方程为 伪距已经过电离层、对流层及卫星钟差改正， 为接收机钟误差 引起的伪距误差及其速度。线性化上式得： 颗卫星。 为接收机至卫星 则 的单位矢量， 为接收机的近似坐标

根据直角坐标与地理坐标的关系 求 得 = ,

对于速度观测量V 对于航向观测量 如观测了n颗卫星，把三个方程合起来写成一个矩阵形式 观测量的协方差阵

二、高精度GPS动态定位和卡尔曼滤波数据处理模型 一般在精度损失较小的前提下对动态模型进行简化。常用的有常速模型 和常加速模型，这两种模型的选择依赖于运动载体的运动状态以及数据 采样率的高低。在高精度GPS动态定经中数据采样率一般为1秒或更高， 此时可采用常加速模型。 1、基本滤波模型 在整周模糊度已知，利用基线初始化或模糊度在航解算OTF求得，观测量 为双差载波相位值，选取GPS接收机天线在WGS-84系中的三维位置 ， 分别为动态接收机天线的三维速度。 （1）状态方程为 动态模型采用常速模型，状态转移矩阵和干扰矩阵定义为：

基准站为R和用户站u在t时刻观测卫星(i,j)这样总共有4个观测量 （2）观测方程 基准站为R和用户站u在t时刻观测卫星(i,j)这样总共有4个观测量 ， 单差，两台接收机在同一历元观测同一卫星的载波相位值相减 站间单差消除了卫星钟差影响

双差：站星载波相位双差观测值为 双差观测值存在数学相关性，若两站同时对6颗卫星进行了观测， 则每个历元在两个测站共有12个载波相位观测值，可组成6个站间 单差观测值，5个站星双差观测值，若在组成双差观测值时选用一 参考卫星，且设站间单差观测值的方差为

如观测了n个卫星，矩阵形式的观测方程可写为 线性化可得： 电离层，对流层廷迟用模型改正后可忽略不计

可采用滤波递推公式

滤波初值为

三、卡尔曼滤波在海上导航及GPS潮位观测中的应用 1、导航滤波 （1）带有白噪声的Kalman滤波模型 该模型是假设船体均匀行驶，扰动加速度ax和ay的期望为0，即: 则定义状态矢量为： 则根据动力学原理有：

（2）有色噪声的Kalman滤波模型 该模型不要求船体匀速假设，加速度为非零均值，且带有一定的观测误差，因而，更接近真实的测量状态。 则定义状态矢量为： 则根据动力学原理有：

（3）导航定位的量测方程 由于不考虑定位方法和定位的构成，x、y直接作为观测值，则相应的观测方程为：

2、GPS潮位数据滤波 在GPS潮位测量中，观测量为单一的高程。 则定义状态矢量为： 则状态方程和观测方程分别为：

四、卡尔曼滤波在姿态数据滤波中的应用 若姿态参数为yaw(y)、roll（r）和Pitch（p），则设状态矢量为： 则状态方程为：

观测方程为：