EKF matlab

来源：互联网发布：如何做淘宝天猫客服编辑：程序博客网时间：2024/04/30 07:38

卡尔曼是真的强，我只能这么说，前面说了卡尔曼滤波，它是针对线性系统的滤波方法。但在实际工作中，我们的系统几乎都是非线性的，所以如何解决非线性系统的滤波问题呢，这就是我们要讲的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滤波在目标跟踪中的应用

%functionEKF_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;%如果增大这个参数，目标的真实轨迹就是曲线了

Q=delta_w*diag([0.5,1]);%过程噪声方差

G=[T^2/2,0;T,0;0,T^2/2;0,T];%过程噪声驱动矩阵

R=5;%观测噪声方差

F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态转移矩阵

x0=200;%观测站的位置

y0=300;

Xstation=[x0,y0];

fort=2:N

X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);%目标真实轨迹

end

for t=1:N

Z(t)=Dist(X(:,t),Xstation)+sqrtm(R)*randn;%观测值

end

%EKF

Xekf=zeros(4,N);

Xekf(:,1)=X(:,1);%Kalman滤波的状态初始化

P0=eye(4);%误差协方差矩阵的初始化

fori=2:N

Xn=F*Xekf(:,i-1);%一步预测

P1=F*P0*F'+G*Q*G';%预测误差协方差

dd=Dist(Xn,Xstation);%观测预测

%求解雅克比矩阵H

H=[(Xn(1,1)-x0)/dd,0,(Xn(3,1)-y0)/dd,0];%泰勒展开的一阶近似

K=P1*H'*inv(H*P1*H'+R);%卡尔曼最优增益

Xekf(:,i)=Xn+K*(Z(:,i)-dd);%状态更新

P0=(eye(4)-K*H)*P1;%滤波误差协方差更新

end

%误差分析

fori=1:N

Err_KalmanFilter(i)=Dist(X(:,i),Xekf(:,i));%

end

%画图

figure

hold on;boxon;

plot(X(1,:),X(3,:),'-k');%真实轨迹

plot(Xekf(1,:),Xekf(3,:),'-r');%扩展Kalman滤波轨迹

legend('real trajectory','EKFtrajectory');

xlabel('X-axis X/m');

ylabel('Y-axisY/m');

figure

hold on;boxon;

plot(Err_KalmanFilter,'-ks','MarkerFace','r')

xlabel('Time/s');

ylabel('Positionestimation bias /m');

%子函数求欧氏距离

function d=Dist(X1,X2);

iflength(X2)<=2

d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);

else

d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);

end

%子函数求欧氏距离

function d=Dist(X1,X2);

iflength(X2)<=2

d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);

else

d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);

end

结果如下：

阅读全文

0 0