加载中…
个人资料
MMFC-little
MMFC-little
  • 博客等级:
  • 博客积分:0
  • 博客访问:75,709
  • 关注人气:13
  • 获赠金笔:0支
  • 赠出金笔:0支
  • 荣誉徽章:
相关博文
推荐博文
谁看过这篇博文
加载中…
正文 字体大小:

扩展卡尔曼滤波EKF及其MATLAB实现

(2016-12-05 12:55:55)
分类: 算法
    卡尔曼是真的强,我只能这么说,前面说了卡尔曼滤波,它是针对线性系统的滤波方法。但在实际工作中,我们的系统几乎都是非线性的,所以如何解决非线性系统 的滤波问题呢,这就是我们要讲的EKF(扩展卡尔曼滤波),它的原理很简单,就是在估计状态的地方进行线性化,线性化的方法也很简单,只需要进行泰勒的一 阶展开就行。当然我们也要说一下缺点,因为我们选择的线性化方法,所以只能达到二阶的精度(UKF可以达到四阶的精度),所以要求我们的系统是弱线性化 的。其实UKF也是对非线性系统的卡尔曼滤波,主要本人对其线性化方法(UT变换)不是很理解,等考完试再说。
  首先,系统的状态方程和观测方程如下:
{\textbf {x}}_{{k}}=f({\textbf {x}}_{{k-1}},{\textbf {u}}_{{k}},{\textbf {w}}_{{k}})
{\textbf {z}}_{{k}}=h({\textbf {x}}_{{k}},{\textbf {v}}_{{k}})
我们知道,在更新误差协方差矩阵的时候,不能直接用f和h的,所以我们要进行泰勒展开,也就是要求雅克比矩阵。再利用线性情况下的卡尔曼滤波进行计算更新。
预测:
{\hat {{\textbf {x}}}}_{{k|k-1}}=f({\textbf {x}}_{{k-1}},{\textbf {u}}_{{k}},0)
{\textbf {P}}_{{k|k-1}}={\textbf {F}}_{{k}}{\textbf {P}}_{{k-1|k-1}}{\textbf {F}}_{{k}}^{{T}}+{\textbf {Q}}_{{k}}
利用雅克比矩阵进行更新模型:

{\textbf {F}}_{{k}}=\left.{\frac {\partial f}{\partial {\textbf {x}}}}\right\vert _{{{\hat {{\textbf {x}}}}_{{k-1|k-1}},{\textbf {u}}_{{k}}}}
{\textbf {H}}_{{k}}=\left.{\frac {\partial h}{\partial {\textbf {x}}}}\right\vert _{{{\hat {{\textbf {x}}}}_{{k|k-1}}}}
更新:
{\tilde {{\textbf {y}}}}_{{k}}={\textbf {z}}_{{k}}-h({\hat {{\textbf {x}}}}_{{k|k-1}},0)
{\textbf {S}}_{{k}}={\textbf {H}}_{{k}}{\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}+{\textbf {R}}_{{k}}
{\textbf {K}}_{{k}}={\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}{\textbf {S}}_{{k}}^{{-1}}
{\hat {{\textbf {x}}}}_{{k|k}}={\hat {{\textbf {x}}}}_{{k|k-1}}+{\textbf {K}}_{{k}}{\tilde {{\textbf {y}}}}_{{k}}
{\textbf {P}}_{{k|k}}=(I-{\textbf {K}}_{{k}}{\textbf {H}}_{{k}}){\textbf {P}}_{{k|k-1}}
下面我会展示一个目标追踪的EKF例子:

12/05/16 01:22:15 /home/fc/桌面/EKF/EKF.m
  
%扩展Kalman滤波在目标跟踪中的应用
  
%function EKF_For_TargetTracking
  
clc;clear;
  
T=1;%雷达扫描周期
  
N=60/T;%总的采样次数
  
X=zeros(4,N);%目标真实位置、速度
  
X(:,1)=[-100,2,200,20];%目标初始位置、速度
  
Z=zeros(1,N);%传感器对位置的观测
  
delta_w=1e-3;%如果增大这个参数,目标的真实轨迹就是曲线了
 10 
Q=delta_w*diag([0.5,1]);%过程噪声方差
 11 
G=[T^2/2,0;T,0;0,T^2/2;0,T];%过程噪声驱动矩阵
 12 
R=5;%观测噪声方差
 13 
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态转移矩阵
 14 
x0=200;%观测站的位置
 15 
y0=300;
 16 
Xstation=[x0,y0];
 17 
for t=2:N
 18 
    X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);%目标真实轨迹
 19 
end
 20 
 
 21 
for t=1:N
 22 
    Z(t)=Dist(X(:,t),Xstation)+sqrtm(R)*randn;%观测值
 23 
end
 24 
%EKF
 25 
Xekf=zeros(4,N);
 26 
Xekf(:,1)=X(:,1);%Kalman滤波的状态初始化
 27 
P0=eye(4);%误差协方差矩阵的初始化
 28 
for i=2:N
 29 
    Xn=F*Xekf(:,i-1);%一步预测
 30 
    P1=F*P0*F'+G*Q*G';%预测误差协方差
 31 
    dd=Dist(Xn,Xstation);%观测预测
 32 
    %求解雅克比矩阵H
 33 
    H=[(Xn(1,1)-x0)/dd,0,(Xn(3,1)-y0)/dd,0];%泰勒展开的一阶近似
 34 
    K=P1*H'*inv(H*P1*H'+R);%卡尔曼最优增益
 35 
    Xekf(:,i)=Xn+K*(Z(:,i)-dd);%状态更新
 36 
    P0=(eye(4)-K*H)*P1;%滤波误差协方差更新
 37 
end
 38 
%误差分析
 39 
for i=1:N
 40 
    Err_KalmanFilter(i)=Dist(X(:,i),Xekf(:,i));%
 41 
end
 42 
%画图
 43 
figure
 44 
hold on;box on;
 45 
plot(X(1,:),X(3,:),'-k');%真实轨迹
 46 
plot(Xekf(1,:),Xekf(3,:),'-r');%扩展Kalman滤波轨迹
 47 
legend(' real trajectory','EKF trajectory');
 48 
xlabel('X-axis  X/m');
 49 
ylabel('Y-axis Y/m');
 50 
 
 51 
figure
 52 
hold on ;box on;
 53 
plot(Err_KalmanFilter,'-ks','MarkerFace','r')
 54 
xlabel('Time /s');
 55 
ylabel('Position estimation bias   /m');
 56 
 
 57 
%子函数 求欧氏距离
 58 
function d=Dist(X1,X2);
 59 
if length(X2)<=2
 60 
    d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
 61 
else
 62 
    d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
 63 
end
 64 
 
 65 
%子函数 求欧氏距离
 66 
function d=Dist(X1,X2);
 67 
if length(X2)<=2
 68 
    d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
 69 
else
 70 
    d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
 71 
end
 72 
 
 73 
 
 74 
 
 75 
 
 76 
 
 77 
 
 78 
 
结果如下:
扩展卡尔曼滤波EKF及其MATLAB实现

扩展卡尔曼滤波EKF及其MATLAB实现


0

阅读 评论 收藏 转载 喜欢 打印举报/Report
  • 评论加载中,请稍候...
发评论

    发评论

    以上网友发言只代表其个人观点,不代表新浪网的观点或立场。

      

    新浪BLOG意见反馈留言板 欢迎批评指正

    新浪简介 | About Sina | 广告服务 | 联系我们 | 招聘信息 | 网站律师 | SINA English | 会员注册 | 产品答疑

    新浪公司 版权所有