第三章 卡尔曼滤波3.1

状态估计算法决定了一个系统的众多参数，KF是导航系统中大多数状态估计算法的基础；

状态估计已成为从各种有效观测数据中获得最优导航结果的关键技术。

系统误差是可复现的，可通过卡尔曼滤波或其他估计算法根据先验信息预测，例如测量中的零偏或常值偏移。

随机误差不可复现也不可预测。

在实际应用中，误差一般即包含系统误差也包含随机误差；例如零偏会发生不可预测的漂移，不过这些可以利用卡尔曼滤波来估计。

3.1 介绍

估计量通过一系列受噪声污染的观测量来更新，而观测量必须是待估计参数的函数。但是，特定时间的观测量集合，不一定足够包含唯一地确定此时待估参数值的信息。

在得到有效信息后，卡尔曼滤波利用系统参数的确定性特性和统计特性等先验知识，以及观测量来获得最优估计，这是一种贝叶斯估计技术。

在提供的初始估计基础上，卡尔曼滤波通过递推算法，用先验值和最新观测数据中得到的新值的加权平均来更新状态估计。

 

为实现数据的加权最优，卡尔曼滤波具有估计的不确定性，而且可给出不同参数估计误差间的相关性度量。这是随着参数估计的逐步迭代实现的，估计过程中也考虑了由于噪声引起的观测值的不确定性。

3.1.1 卡尔曼滤波的要素

五个核心要素：状态向量及其协方差、系统模型、观测向量及其协方差、观测模型和滤波算法。

 状态向量（state vector）是一组描述系统的参数，又称为状态，是卡尔曼滤波估计的对象。状态可能是常量也可能是时变量。

大多数导航应用中，状态量一般包括位置或者位置误差，速度、姿态以及导航传感器的误差也可作为被估状态。

与状态向量相关联的是误差协方差矩阵（error covariance matrix）。该矩阵描述了卡尔曼滤波状态估计的不确定度及估计误差之间的相关程度。

误差协方差矩阵中的相关信息非常重要，原因有三：1、它可完全表示状态估计的误差分布。如下图所示，北向和东向位置估计所示，当不考虑相关性时，会导致一个方向过高估计而另一个方向过低估计。

 2、观测量中往往没有足够的信息来独立地估计卡尔曼滤波的状态，相关信息使得在等待新的观测信息时，可估计那些需保持的状态间的线性关系。

3、误差间的相关信息可以建立起观测量之间的联系。

通过建立模型可使得某些状态可由其他状态确定（例如从一系列的位置中确定速度）。

卡尔曼滤波是一个迭代过程，故状态向量和协方差矩阵的初值需由用户设定或由其他过程确定。

系统模型（system model），也被称为过程模型或者时间传播模型，描述了卡尔曼滤波的状态及误差协方差矩阵随时间的变化特性。系统模型对状态来说是确定的，因为它是基于系统的已知特性建立起来的。这种与状态真值间的偏差被称作系统噪声（system noise）或者过程噪声，其统计特性一般由卡尔曼滤波设计者定义。

观测向量（measurement vector）是一组针对同一时刻的系统特性的观测值，是状态向量的函数。系统初始化后，所有状态估计值便可从这些信息中获得。与观测向量相对应的是观测噪声协方差矩阵（measurement noise covariance），它描述了观测噪声的统计特性。

观测模型（measurement model）描述了在没有观测噪声的情况下，观测向量作为真实状态向量（区别于状态向量估计值）的函数是怎样变化的。

卡尔曼滤波算法使用观测向量、观测模型和系统模型来获得状态向量的最优估计。

 

 

3.1.2 卡尔曼滤波的流程

算法由两个流程构成：系统传播流程和观测更新流程，两个流程每次迭代由10个步骤组成，如下图，

 步骤 1 -4 是系统传播流程,步骤5 -10是卡尔曼滤波的观测更新流程。每一次卡尔曼滤波迭代的完成,都是相对于特定的时间点而言的,一次迭代称为一个"节拍" 。

 系统传播（或称为时间传播）流程的目的，是根据已知的系统特性，从上一组观测量修正后的时刻推进到当前一组观测量的时刻，前向预测状态向量估计与误差协方差矩阵。当前时刻，在没有最新观测信息的情况下，系统传播提供了状态向量的卡尔曼滤波最优估计。前两个步骤计算了系统模型的确定性部分和噪声部分。第三步为状态传播（state propagation），利用该步实现状态向量估计的更新。第四步为协方差传播（covariance propatation），根据系统噪声引起的状态不确定度的增加，更新误差协方差矩阵。

在观测更新（或修正）流程中，用最新观测信息更新状态向量估计和误差协方差矩阵。第五步和第六步计算观测模型的确定性部分和噪声部分。第七步，增益计算（gain computation），计算卡尔曼滤波的增益矩阵。此步根据当前状态估计的不确定度和观测噪声情况，对状态的修正量进行优化加权。第八步构建观测向量。第九步，观测更新（measurement update），用卡尔曼增益加权的观测数据更新状态估计。最后，协方差更新（covariance update），用已包含在状态向量估计值中的观测数据新信息来更新误差协方差矩阵。

下图定性地说明了卡尔曼滤波如何从连续的不完整观测值中确定位置解：

在节拍1，初始的二维位置不确定度很大，在该节拍的有效观测量是一条位置线（line of position，LOP），该位置线可能来自于远程发射机或方位角测量仪等距离测量设备。观测量只提供了与位置线正交方向上的位置信息，并不能由此得到唯一的定位点信息。通过一次卡尔曼滤波观测更新，使得位置估计值更接近观测线，在与观测位置线正交方向上的位置不确定度大大降低，但沿位置线方向并没有降低。

在节拍2，通过卡尔曼滤波系统传播流程，利用物体可能存在的移动信息，降低了位置不确定度。在节拍2的有效观察量也是一条位置线，但与第一条方向不同，因此提供了与第一次观测不同方向的位置信息。这样，通过卡尔曼滤波观测更新，就可以得到在两个方向上不确定度很小的位置估计值。

 3.1.3 卡尔曼滤波的应用

对单独的GNSS导航，估计的状态为用户天线的位置、速度、接收机时钟偏差和时钟漂移，观测量是由每个接收机产生的各卫星信号视线距离测量值。