扩展卡尔曼滤波

这里必须保证Xk-1，Xk，Xk+1都是正态分布，否则的话我们就没法递推了，如果Xk不是正态分布的话就没有办法调用我们扩展卡尔曼滤波的算法，自然就会引起误差，因为本质上f和h是非线性函数，所以说如果是这个真实的后验概率呢他一定不是这个正态分布，因为他是非线性函数，但是我们滤波的结果呢却是正态分布，所以说他就是一种误差，误差就在这体现，所以说扩展卡尔曼滤波是一种近似，这种近似是有误差的，误差来源就在这


预测步和更新步泰勒展开的展开点不同，
预测步是在Xk-1plus展开
更新步是在Xkminus展开






扩展卡尔曼滤波的算法和卡尔曼滤波的算法很像，最大的不一样就是他要求一个雅克比矩阵，第二个不一样就是算法的2 7步，这里是
卡尔曼是Xkminus=A*Xk-1plus,也就是说在这里他并没有线性化，还有第7步

卡尔曼是Xkplus=C*Xkminus，下面来算一个C帮助大家理解雅克比矩阵

以上是预测
以下是观测

这样呢ekf的理论就讲完了，接下来是代码

%EKF代码
%纸上得来终觉浅，绝知此事要躬行%x(k)=sin(3x(k-1))
%y(k)=x(k)'2
%注意似然概率是多峰分布，具有强烈的非线性，当y=4时，不知道x=2还是-2
%%%生成真实信号与观测
t=0.01:0.01:1;
n=length(t);
X=zeros(1,n);
y=zeros(1,n);
x(1)=0.1;
y(1)=0.1^2;
for i=2:n
x(i)=sin(3x(i-1));
y(i)=x(i)^2+normrnd(0,0.1);
end
plot(t,x,‘r’,t,y,‘b’,‘LineWidth’,4)

红色代表真实的信号，蓝色代表观测

%EKF代码
%纸上得来终觉浅，绝知此事要躬行%x(k)=sin(3x(k-1))
%y(k)=x(k)'2
%注意似然概率是多峰分布，具有强烈的非线性，当y=4时，不知道x=2还是-2
%%%生成真实信号与观测
t=0.01:0.01:1;
n=length(t);
X=zeros(1,n);
y=zeros(1,n);
x(1)=0.1;
y(1)=0.1^2;
for i=2:n
x(i)=sin(3x(i-1));
y(i)=x(i)^2+normrnd(0,0.7);
end
%EKF
Xplus=zeros(1,n);%设置初值
Pplus=0.1;
Xplus(1)=0.1;
Q=0.1;
R=1;
for i=2:n
%预测步
A=3cos(3Xplus(i-1));
Xminus=sin(3Xplus(i-1));
Pminus=APplusA’+Q;
%更新步
C=2Xminus;
K=PminusCinv(CPminusC’+R);
Xplus(i)=Xminus+K*(y(i)-Xminus^2);
Pplus=(eye(1)-K*C)*Pminus;
end

plot(t,x,‘r’,t,Xplus,‘b’,‘LineWidth’,4)

结果是比较差的，但也算符合我们的预期，我们知道ekf对强烈的非线性问题处理的不太好，他的观测噪声比较大，所以效果不好，我们看一下能不能抢救一下，
Q=0.0001;以后长这样

结果还是比较差，比刚才那个稍微好一些，而且有一种很强烈的多峰分布的一个感觉，是因为ekf直接把函数线性化了，这样的话对强非线性问题算的就不是特别的好，如果对一些比较弱的非线性问题他有可能会得到一个比较好的结果，像我们这个例子他的观测噪声比较大，他的似然概率又是一个多峰分布，这样的话就会很差，如果遇到一些强烈的非线性问题的话，扩展卡尔曼滤波需要花很多精力去调Q和R才能得到一个理想的结果，而且一般来说是比较难调的，这就是ekf自然的缺陷，他的优点就在于快，计算速度非常快，和kf差不多，需要的内存也比较小，世上第一个可以进行实时计算的slam程序就是用ekf写出来的，不过呢现在slam都在用最线性优化，第九讲提到了slam和相关的算法，ekf也是人类发明出来的第一个处理非线性问题的kf，对一些实时性要求过高并且非线性程度比较弱的场景中ekf也是经常被用到的，ekf到此为止，最后只剩一个ukf（无迹卡尔曼滤波）。