卡尔曼滤波最新的改进滤波算法

A. 扩展卡尔曼滤波（EKF）算法详细推导及仿真（Matlab）

姓名：王柯祎

学号：20021110373T

转自 ：https://blog.csdn.net/gangdanerya/article/details/105105611

【嵌牛导读】介绍扩展卡尔曼滤波（EKF）算法的详细推导，局限性和MATLAB仿真。

【嵌牛鼻子】扩展卡尔曼滤波（EKF）

【嵌牛正文】

扩展卡尔曼滤波算法 是解决非线性状态估计问题最为直接的一种处理方法，尽管EKF不是最精确的”最优“滤波器，但在过去的几十年成功地应用到许多非线性系统中。所以在学习非线性滤波问题时应该先从EKF开始。

EKF算法是将非线性函数进行泰勒展开，然后省略高阶项，保留展开项的一阶项，以此来实现非线性函数线性化，最后通过卡尔曼滤波算法近似计算系统的状态估计值和方差估计值。

一、EKF算法详细推导

【注】EKF推导参考的是黄蔚的博士论文“CKF及鲁棒滤波在飞行器姿态估计中的应用研究”，论文中EKF，UKF和CKF等算法讲解的都很详细，值得一看。

我们把KF与EKF算法拿出来对比可以发现：

二、EKF算法局限性：

该算法线性化会引入阶段误差从而导致滤波精度下降，同时当初始状态误差较大或系统模型非线性程度较高时，滤波精度会受到严重影响甚至发散。

需要计算雅克比矩阵，复杂，计算量大，影响系统的实时性，还会导致EKF算法的数值稳定性差。

当系统存在模型失配，量测干扰，量测丢失，量测延迟或状态突变等复杂情况时，EKF算法鲁棒性差。

三、Matlab仿真：

clear all;clc;   close all;

tf = 50; 

Q = 10;w=sqrt(Q)*randn(1,tf); 

R = 1;v=sqrt(R)*randn(1,tf);

P =eye(1);

x=zeros(1,tf);

Xnew=zeros(1,tf);

x(1,1)=0.1; 

Xnew(1,1)=x(1,1);

z=zeros(1,tf);

z(1)=x(1,1)^2/20+v(1);

zjian=zeros(1,tf);

zjian(1,1)=z(1);

for k = 2 : tf

%%%%%%%%%%%%%%%模拟系统%%%%%%%%%%%%%%%

    x(:,k) = 0.5 * x(:,k-1) + (2.5 * x(:,k-1) / (1 + x(:,k-1).^2)) + 8 * cos(1.2*(k-1)) + w(k-1); 

    z(k) = x(:,k).^2 / 20 + v(k);

%%%%%%%%%%%%%%%EKF开始%%%%%%%%%%%%%%%

    Xpre = 0.5*Xnew(:,k-1)+ 2.5*Xnew(:,k-1)/(1+Xnew(:,k-1).^2) + 8 * cos(1.2*(k-1));  

    zjian =Xpre.^2/20;

    F = 0.5 + 2.5 * (1-Xnew.^2)/((1+Xnew.^2).^2);

    H = Xpre/10;    

    PP=F*P*F'+Q; 

    Kk=PP*H'*inv(H*PP*H'+R);

    Xnew(k)=Xpre+Kk*(z(k)-zjian);

    P=PP-Kk*H*PP;

end

  t = 2 : tf;  

 figure;   plot(t,x(1,t),'b',t,Xnew(1,t),'r*');  legend('真实值','EKF估计值');

仿真结果：

B. 卡尔曼滤波本质是一种滤波方式还有什么其他方法和卡尔曼滤波一样的方法

卡尔曼滤波的一个典型实例是从一组有限的，对物体位置的，包含噪声的观察序列预测出物体的坐标位置及速度. 在很多工程应用(雷达, 计算机视觉)中都可以找到它的身影. 同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要话题.状态估计是卡尔曼滤波的重要组成部分。一般来说，根据观测数据对随机量进行定量推断就是估计问题，特别是对动态行为的状态估计，它能实现实时运行状态的估计和预测功能。比如对飞行器状态估计。状态估计对于了解和控制一个系统具有重要意义，所应用的方法属于统计学中的估计理论。最常用的是最小二乘估计，线性最小方差估计、最小方差估计、递推最小二乘估计等。其他如风险准则的贝叶斯估计、最大似然估计、随机逼近等方法也都有应用。
受噪声干扰的状态量是个随机量，不可能测得精确值，但可对它进行一系列观测，并依据一组观测值，按某种统计观点对它进行估计。使估计值尽可能准确地接近真实值，这就是最优估计。真实值与估计值之差称为估计误差。若估计值的数学期望与真实值相等，这种估计称为无偏估计。卡尔曼提出的递推最优估计理论，采用状态空间描述法，在算法采用递推形式，卡尔曼滤波能处理多维和非平稳的随机过程。
卡尔曼滤波理论的提出，克服了威纳滤波理论的局限性使其在工程上得到了广泛的应用，尤其在控制、制导、导航、通讯等现代工程方面。

C. 什么叫卡尔曼滤波算法其序贯算法

卡尔曼滤波算法（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
序贯算法又叫序贯相似性检测算法，是指图像匹配技术是根据已知的图像模块(模板图)在另一幅图像(搜索图)中寻找相应或相近模块的过程，它是计算机视觉和模式识别中的基本手段。已在卫星遥感、空间飞行器的自动导航、机器人视觉、气象云图分析及医学x射线图片处理等许多领域中得到了广泛的应用。研究表明，图像匹配的速度主要取决于匹配算法的搜索策略。
数据滤波是去除噪声还原真实数据的一种数据处理技术, Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态. 由于, 它便于计算机编程实现, 并能够对现场采集的数据进行实时的更新和处理, Kalman滤波是目前应用最为广泛的滤波方法, 在通信, 导航, 制导与控制等多领域得到了较好的应用。

D. kalman滤波有哪些改进办法

扩展Kalman滤波，无际Kalman滤波，集合kalman滤波，自适应kalman滤波，强跟踪Kalman滤波

E. 哪本书讲卡尔曼滤波讲得好

最经典的当属“ 西北工大出版社出版的，秦永元的《卡尔曼滤波与组合导航原理》”

是想研究卡尔曼滤波的入门教材。

F. 卡尔曼滤波理解与实现

本文为离散卡尔曼滤波算法的一 一个简明教程，从算法思想、实现过程、理论推导和程序实现四个方面阐述和分析了卡尔曼滤波算法。

XU Ruilin完成本教程主要部分的编写，WANG Xuejun完成第3节的编写，ZHU Ximin完成2.2节的编写，WEN Shuhan完成2.3节的编写，MAO Bo完成全文整理、修订和排版。

卡尔曼滤波（Kalman Filtering）及其一系列的优化和改进算法是目前在求解运动状态推算问题上最为普遍和高效的方法。 鲁道夫·卡尔曼 （Rudolf Emil Kalman） 在NASA埃姆斯研究中心访问时，发现他的方法适用于解决阿波罗计划的轨迹预测问题。阿波罗飞船的导航电脑就是使用这种滤波器进行轨迹预测。

卡尔曼滤波尤其适用于动态系统，这种方法对于内存要求极低而运算速度快，且能够保持较好的计算精度，这使得这种方法非常适合解决实时问题和应用于嵌入式系统，也就是说，卡尔曼滤波天然的适用于解决舰艇指控系统的航迹推算问题。在接下来的内容里，我们将逐步领会卡尔曼滤波的这些绝佳特点。

不过，现在我们先从复杂的舰艇航迹推算问题中解脱出来，从一个更加熟悉和简单的问题中来理解这个滤波算法的思想、过程和算法。

假设有一辆无人车WALL-E，需要导引它从A点到达B点，共有两种手段（ 图1 ）：

显然，两种方法都有一定的误差。如果单独采用某一种方法进行定位，WALL-E在误差的影响下将无法到达B点。因此，需要将两种方法结合起来，得到一个更加精确的结果，这就是卡尔曼滤波要解决的问题。

卡尔曼滤波方法如何看待我们的问题呢？在探究这个问题之前，我们先对问题进行抽象，并用数学语言来描述我们的问题。

我们用矢量 来描述WALL-E的运动状态，这个列矢量 包括位置矢量 和速度矢量 两个分量。在WALL-E的问题上，我们现在不知道位置 和速度 的准确值，但是知道WALL-E的运动模型满足 状态方程 ，定位的方法，也即观测WALL-E运动状态的方法满足 观测方程 . 当然，我们也知道，这两种方法都存在一定的误差 ，那么我们的问题就可以转化为一个优化问题——

在这一优化问题中，目标函数是要使预测（估计）误差最小，同时约束于估计方法 和 的条件下。在卡尔曼滤波中，我们的估计原则（也就是最小化估计误差的原则）是 最小方差无偏估计 [1] ，我们将通过后面的过程分析来说明这一点。

在我们正式开始引入公式分析卡尔曼滤波问题之前，我们还必须解决一个问题------把连续的线性系统离散化，也就是将连续时域问题转化为时间序列问题。当然，目前我们只讨论线性系统的情况，关于非线性系统问题，我们有扩展卡尔曼滤波（Extended Kalman Filtering, EKF）和无迹卡尔曼滤波（Unscented Kalman Filtering, UKF）两种方法来求解。

补充内容------连续线性时变系统的离散化
设连续线性时变系统的时域状态方程为

若采样周期为 ，则从时刻 到时刻 ，有

令 ， ，则离散化的状态方程为

通过对线性系统的离散化处理，我们现在可以考虑每一个时刻WALL-E的运动状态。接下来，我们将用 来表示在 时刻运动状态的最优估计值；用 表示用 时刻对 时刻的状态预测值；用 表示对 时刻综合预测和观测两种方法的最优估计值。

在估计WALL-E位置的问题上，假定我们已经知道它是匀速直线运动，WALL-E身上还携带有一个GPS传感器可以提供它的位置信息，WALL-E在前进过程中可能会遇到一些情况，比如停止前进或是受到风的影响。

加入我们已知的是WALL-E上一个时刻的最佳估计状态，即k-1时刻的位置和速度，要求的是下一时刻即k时刻的最佳估计状态，即k时刻的位置和速度，我们可以发现有两种方法可以得到它的k时刻的状态：

一种是通过WALL-E设定程序计算得到下一秒的状态，比如现在设定是匀速直线运动，那么下一秒的速度应该是恒定不变的，而位置则是在上一秒位置的基础上加上时间乘以速度即一秒内走过的路程，但是现实生活中并不是理想的，机器人会受到摩擦力、风力等的影响，当然也可能会有顽皮的小孩挡住他前进的道路，这些因素使得WALL-E在k时的真实状态与我们计算得到的数据有所不同。

另一种是通过WALL-E所携带的GPS来确定它的位置，因为GPS是测量出的就是WALL-E的实时状态，因此它比较准确。但是GPS测量k时刻的状态有两个问题，一是GPS只能测出WALL-E的位置，而测不出它的速度；二是GPS传感器测量的时候也会有仪器的误差，只能说它是比较准确的，比较接近真实值的。

那么接下来问题来了，我们如何得到k时刻WALL-E的真实状态呢？

我们将第一种方法得到的状态值称为预测值，第二种方法得到的状态值称为测量值，对汽车的最佳估计就是将这两部分信息结合起来，尽量的去逼近k时刻的真实值。

下面再深入一些思考，怎么将这两部分结合起来？

在初始时间k-1， 是WALL-E的最佳估计值，WALL-E其实可以是估计值附近的任何位置，并且这种不确定性由该概率密度函数描述。WALL-E最有可能在这个分布的平均值附近。在下一个时间，估计的不确定性增加，用一个更大的方差表示，这是因为在时间步骤k-1和k之间，WALL-E可能收到了风力的影响，或者脚可能向前滑了一点，因此，它可能已经行进了与模型预测的距离不同的距离。

WALL-E位置的另一个信息来源来自测量，方差表示误差测量的不确定性，真正的位置同样可以是平均值附近的任何位置。

预测值和测量值，对WALL-E的最佳估计是将这两部分信息结合起来，将两个概率函数相乘得到另一个高斯函数，该估计值的方差小于先前估计值，并且该概率密度函数的平均值为我们提供了WALL-E位置的最佳估计。

以下，我们将进行e的运算推导

设：

则有实际目标变量的表达式：

数学模型中目标变量的表达式：

实际模型中测量变量的表达式：

数学模型中测量变量的表达式：

将目标变量的实际值和估计值相减：

将上述方程带入误差e的表达式，我们可得出误差e的解析解：

从推导结果中我们不难看出，估计值和实际值的误差随时间呈指数形式变化，当（F-KH）<1时，随着时间的推移，会无限趋近于零，也就是意味着估计值和实际值相吻合。这就是为什么卡尔曼滤波器可以完美预测出目标状态值的原理。

在估计WALL-E位置的问题上，我们不知道位置 和速度 的准确值，但是我们可以给出一个估计区间（ 图5.a ）。卡尔曼滤波假设所有的变量是随机的且符合高斯分布（正态分布）。每个变量有一个均值 和一个方差 （ 图5.b ）。而 图5.c 则表示速度和位置是相关的。

假如我们已知上一个状态的位置值，现在要预测下一个状态的位置值。如果我们的速度值很高，我们移动的距离会远一点。相反，如果速度慢，WALL-E不会走的很远。这种关系在跟踪系统状态时很重要，它给了我们更多的信息：一个观测值告诉我们另一个观测值可能是什么样子。这就是卡尔曼滤波的目的------从所有不确定信息中提取有价值的信息。

根据数理统计知识，我们知道这种两个观测值（随机变量）之间的关系可以通过一个协方差矩阵

描述（ 图6 ）。

我们假设系统状态的分布为 高斯分布（正态分布） ，所以在 时刻我们需要两个信息：最佳预估值 及其协方差矩阵 （如式(2)所示）。

下一步，我们需要通过 时刻的状态来预测 时刻的状态。请注意，我们不知道状态的准确值，但是我们的预测函数并不在乎，它仅仅是对 时刻所有可能值的范围进行预测转移，然后得出一个k时刻新值的范围。在这个过程中，位置 和速度 的变化为

我们可以通过一个状态转移矩阵 来描述这个转换关系

同理，我们更新协方差矩阵 为

到目前为止，我们考虑的都是匀速运动的情况，也就是系统没有对WALL-E的运动状态进行控制的情况。那么，如果系统对WALL-E进行控制，例如发出一些指令启动或者制动轮子，对这些额外的信息，我们可以通过一个向量 来描述这些信息，并将其添加到我们的预测方程里作为一个修正。假如我们通过发出的指令得到预期的加速度 ，运动状态方程就更新为

引入矩阵表示为

式中 称为控制矩阵， 称为控制向量（例如加速度 ）。当然，如果没有任何外界动力影响的系统，可以忽略这一部分。

我们增加另一个细节，假如我们的预测转换矩阵不是100%准确呢，会发生什么？如果状态只会根据系统自身特性演变，那样将不会有任何问题。如果所有外界作用力对系统的影响可以被计算得十分准确，那样也不会有任何问题。但是如果有些外力我们无法预测，例如我们在跟踪一个四轴飞行器，它会受到风力影响；或者在跟踪一个轮式机器人，轮子可能会打滑，地面上的突起会使它减速。我们无法跟踪这些因素，而这些不确定事件发生时，预测方程将会失灵。因此，我们将这些不确定性统一建模，在预测方程中增加一个不确定项。

通过这种方式，使得原始状态中的每一个点可以都会预测转换到一个范围，而不是某个确定的点（ 图7.a ）。 可以这样描述------ 中的每个点移动到一个符合方差 的高斯分布里（ 图7.b ）。换言之，我们把这些不确定因素描述为方差为 的高斯噪声，并用 表示。这样就会产生一个新的高斯分布，方差不同，但是均值相同（ 图7.c ）。

通过对 的叠加扩展，得到完整的预测转换方程为

新的预测转换方程只是引入了已知的系统控制因素。新的不确定性可以通过之前的不确定性计算得到。到这里，我们得到了一个模糊的估计范围------通过 和 描述的范围。

我们之前的工作仍然是在使用运动模型一种方法来估计系统的状态，现在，我们要把另一种方法，也就是观测（本问题中为GPS定位）考虑进来，以进一步修正对运动状态的估计（ 图8 ）。

我们用矩阵 来描述观测方法的作用，于是有

再加入观测噪声 ，观测方程为

从控制论的角度出发，我们定义新息（也即观测值与预测值的误差）为

当然我们也知道，观测本身也会存在误差，比如本问题中的GPS定位精度仅有10m. 因此，我们用矩阵 来描述这种不确定性（ 图10 及 图11.a ）。

这时，我们新息的协方差为

现在我们需要把两种方法得到的可能性融合起来（ 图11.b ）。对于任何状态，有两个可能性：1. 传感器的观测值更接近系统真实状态；2. 模型推算的估计值更接近系统真实状态。如果有两个相互独立的获取系统状态的方式，并且我们想知道两者都准确的概率值，于是我们可以通过加权来解决更相信谁的问题（ 图11.c ）。

我们现在知道，系统模型的状态预测 与对系统的状态观测 服从高斯分布，把这个问题抽象一下就是——

根据我们的一个估计准则------ 最小方差估计 ，那么这个问题可以转化为优化问题求解

求导数（差分）得

则 ，从而

当维度高于一维时，我们用矩阵来描述，有

这里的 称为 卡尔曼增益 （Kalman Gain），也就是我们在解决更信任哪种方法时的偏向程度。

如果我们从两个独立的维度估计系统状态，那么根据系统模型的预测为

通过传感器的观测为

我们结合着两种方法得到

由 可知，卡尔曼增益为

将 约去（ 中也含有 项），得

此时的卡尔曼增益实际为

我们最后再来验证一下 估计的无偏性 ——

这里我们设 时刻的真值为 ，由于

由于 （ 从初值而来的无偏传递性 ）可知 ，即卡尔曼滤波满足无偏估计准则。显然，其中要求系统噪声和观测噪声是不相关、零期望的白噪声，且是线性系统，初始时刻的状态估计是无偏的。当这些条件不能满足时，卡尔曼滤波的估计结果是有偏的。

到这里，我们已经获得了卡尔曼滤波的全部要素。我们可以把整个过程总结为3个基本假设

假设一 和 都是零均值高斯白噪声，也即 ，

假设二 与 无关，也即

假设三 系统初值 的均值和方差已知，且 与 均不相关。

以及5个基本方程 方程一 状态预测

方程二 协方差预测

方程三 卡尔曼增益

G. 卡尔曼滤波的基本原理和算法

卡尔曼滤波的原理用几何方法来解释。这时，~X和~Z矩阵中的每个元素应看做向量空间中的一个向量而不再是一个单纯的数。这个向量空间（统计测试空间）可以看成无穷多维的，每一个维对应一个可能的状态。~X和~Z矩阵中的每个元素向量都是由所有可能的状态按照各自出现的概率组合而成（在测量之前，~X和~Z 的实际值都是不可知的）。~X和~Z中的每个元素向量都应是0均值的，与自己的内积就是他们的协方差矩阵。无法给出~X和~Z中每个元素向量的具体表达，但通过协方差矩阵就可以知道所有元素向量的模长，以及相互之间的夹角（从内积计算）。
为了方便用几何方法解释，假设状态变量X是一个1行1列的矩阵（即只有一个待测状态量），而量测变量Z是一个2行1列的矩阵（即有两个测量仪器，共同测量同一个状态量X），也就是说，m=1，n=2。矩阵X中只有X[1]一项，矩阵Z中有Z[1]和Z[2]两项。Kg此时应是一个1行2列的矩阵，两个元素分别记作Kg1 和 Kg2 。H和V此时应是一个2行1列的矩阵。

参考资料：
http://blog.csdn.net/newthinker_wei/article/details/11768443

H. 无味卡尔曼滤波与扩展卡尔曼滤波的具体区别，以及算法

EKF是对非线性系统模型（方程）进行的线性化近似，以利用KF算法进行滤波估计。而UKF是对状态的概率统计近似，即设计少量的σ点，由σ点经由非线性函数的传播，计算出随机向量一、二阶统计特性的传播，对于高斯噪声的假设，UKF能够达到三阶估计精度，而EKF只能达到二阶精度，但其算法仍然是利用KF的算法。
现在国内外的文献大都是对UKF算法的改进和应用进行论述，但对算法的稳定性等没有系统的论述。我了解得沈阳自动化所做的这方面的工作很多。

I. 卡尔曼滤波算法是什么

卡尔曼滤波是一个滤波算法，应用非常广泛，它是一种结合先验经验、测量更新的状态估计算法，卡尔曼滤波器是在估计线性系统状态的过程中，以最小均方误差为目的而推导出的几个递推数学等式。

卡尔曼过程中要用到的概念。即什么是协方差，它有什么含义，以及什么叫最小均方误差估计，什么是多元高斯分布。如果对这些有了了解，可以跳过，直接到下面的分割线。

均方误差：

它是"误差"的平方的期望值（误差就是每个估计值与真实值的差），也就是多个样本的时候，均方误差等于每个样本的误差平方再乘以该样本出现的概率的和。

方差：

方差是描述随机变量的离散程度，是变量离期望值的距离。

注意:

两者概念上稍有差别，当你的样本期望值就是真实值时，两者又完全相同。最小均方误差估计就是指估计参数时要使得估计出来的模型和真实值之间的误差平方期望值最小。

J. 卡尔曼滤波算法的发展历史如何

全球定位系统（GPS）是新一代的精密卫星导航定位系统。由于其全球性、全天候以及连续实时三维定位等特点，在军事和民用领域得到了广泛的发展。近年来，随着科学技术的发展，GPS导航和定位技术已向高精度、高动态的方向发展。但是由于GPS定位包含许多误差源，尤其是测量随机误差和卫星的几何位置误差，使定位精度受到影响。利用传统的方法很难消除。而GPS动态滤波是消除GPS定位随机误差的重要方法，即利用特定的滤波方法消除各种随机误差，从而提高GPS导航定位精度。 经典的最优滤波包括：Wiener滤波和Kalman滤波。由于Wiener滤波采用频域法，作用受到限制；而Kalman滤波采用时域状态空间法，适合于多变量系统和时变系统及非平稳随机过程，且由于其递推特点容易在计算机上实现，因此得到了广泛的应用。为此，本文对Kalman滤波方法进行了深入的研究，并取得了一些成果。 本文首先概述了GPS的组成、应用及最新动态。在此基础上介绍了GPS的导航定位原理，给出了卫星可见性算法、选星算法及定位算法。然后介绍了卡尔曼滤波的基本原理，在此基础上对动态用户的飞行轨迹进行了仿真，对“singer”模型下的8状态和11状态卡尔曼滤波算法进行了仿真分析，同时对“当前”统计模型下11状态卡尔曼滤波算法进行了仿真分析，并对滤波前后的定位精度进行了比较。在此基础上，就如何提高滤波器的动态性能作者提出了改进算法，即自适应卡尔曼滤波算法、带渐消因子的优化算法及改进的优化算法，并分别进行了仿真分析。最后作者将卡尔曼滤波算法分别应用于GPS/DR和GPS/INS组合导航定位系统中，并分别对这两种系统进行了建模和仿真分析，取得了较理想的结果。 本文的研究工作，对改进传统的滤波方法有一定的参考和应用价值，并对卡尔曼滤波方法在提高GPS动态导航定位精度方面的应用起到积极的促进作用。