扩展卡尔曼滤波（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估世厅计值');
仿真结果：
基于卡尔曼滤波的目标跟踪怎么做？
在cnki上下篇kalman目标跟踪的硕士论文吧，很多的，当然期刊也可以，不过一般情况下硕士论文讲的能详细点，然后找准一篇仔细研读，这样子基本上理论就没啥问题了，编程就用MATLAB，用C很麻烦，很多算法都没有得自己从头编，matlab集成了很多的算法的，只要找出来调用就行了。
这里给你说下kalman跟踪的思路吧：
0.如果你的视频是实际录得话，为防止检测到伪目标，首先要对输入的图像进行滤波，简单的有中值均值滤波。
1.对视频序列采用背景差分或帧间差分就可以得到运动区域了，这里重点就是背景建模，如果嫌麻烦也就别看什么单高斯或多高斯的了，直接找一个空帧(没有运动目标)当背景就OK了，差分后就有了运动区域，然后二值化方便以后的处理。然后视有没有阴影而进行阴影去除的工作。
2.上边这步也就是检测出了运动区域，按你的检测出来是要给边边画圈，这个在matlab上好好研究研究怎样提取目标边缘的点，在原位图图上把边缘的点改变成一个同像素值就行了，这样检测就完了。
3.跟踪，首先得找到目标的中心，因为目标不只是一个像素，必须有一个中心来表示它的坐标位置，这个方法自己想啦，什么取均值求外接矩形中心啊都可以的，然后每一帧都这么做就有一系列的中心坐标了。
4.Kalman，Kalman的作用还是以滤波为主，相当于把第三步的那些坐标都当成信号序列，用Kalman滤波，边检测边滤波，kalman主要记住那5个公式，知道它的递推过程就基本能编出来了，至于滤波器参数就在参考文献里找吧，编出来kalman部分的程序没多少行的，别怕。
5.如果是多目标跟踪的话就进行目标匹配的工作，相当于每帧都检测出两个目标，你要知道最新一帧中的每个分别对应的是前边帧的哪个目标。
上边这些给你一个大体的思路，你根据自己的任务选择做哪些工作，这个题目不难的，要有信心
