扩展卡尔曼滤波(EKF)仿真演示.doc
- 文档编号:18721879
- 上传时间:2023-10-19
- 格式:DOC
- 页数:4
- 大小:178KB
扩展卡尔曼滤波(EKF)仿真演示.doc
《扩展卡尔曼滤波(EKF)仿真演示.doc》由会员分享,可在线阅读,更多相关《扩展卡尔曼滤波(EKF)仿真演示.doc(4页珍藏版)》请在冰点文库上搜索。
精品文档
扩展卡尔曼滤波(EKF)仿真演示
(西工大严恭敏,2012-2-4)
一、问题描述
如图1所示,从空中水平抛射出的物体,初始水平速度,初始位置坐标();受重力和阻尼力影响,阻尼力与速度平方成正比,水平和垂直阻尼系数分别为;还存在不确定的零均值白噪声干扰力和。
在坐标原点处有一观测设备(不妨想象成雷达),可测得距离(零均值白噪声误差)、角度(零均值白噪声误差)。
图1雷达观测示意图
二、建模
系统方程:
量测方程:
选状态向量,量测向量
系统Jacobian矩阵
量测Jacobian矩阵
三、Matlab仿真
functiontest_ekf
kx=.01;ky=.05;%阻尼系数
g=9.8;%重力
t=10;%仿真时间
Ts=0.1;%采样周期
len=fix(t/Ts);%仿真步数
%真实轨迹模拟
dax=1.5;day=1.5;%系统噪声
X=zeros(len,4);X(1,:
)=[0,50,500,0];%状态模拟的初值
fork=2:
len
x=X(k-1,1);vx=X(k-1,2);y=X(k-1,3);vy=X(k-1,4);
x=x+vx*Ts;
vx=vx+(-kx*vx^2+dax*randn(1,1))*Ts;
y=y+vy*Ts;
vy=vy+(ky*vy^2-g+day*randn
(1))*Ts;
X(k,:
)=[x,vx,y,vy];
end
figure
(1),holdoff,plot(X(:
1),X(:
3),'-b'),gridon
%figure
(2),plot(X(:
2:
2:
4))
%构造量测量
mrad=0.001;
dr=10;dafa=10*mrad;%量测噪声
fork=1:
len
r=sqrt(X(k,1)^2+X(k,3)^2)+dr*randn(1,1);
a=atan(X(k,1)/X(k,3))+dafa*randn(1,1);
Z(k,:
)=[r,a];
end
figure
(1),holdon,plot(Z(:
1).*sin(Z(:
2)),Z(:
1).*cos(Z(:
2)),'*')
%ekf滤波
Qk=diag([0;dax;0;day])^2;
Rk=diag([dr;dafa])^2;
Xk=zeros(4,1);
Pk=100*eye(4);
X_est=X;
fork=1:
len
Ft=JacobianF(X(k,:
),kx,ky,g);
Hk=JacobianH(X(k,:
));
fX=fff(X(k,:
),kx,ky,g,Ts);
hfX=hhh(fX,Ts);
[Xk,Pk,Kk]=ekf(eye(4)+Ft*Ts,Qk,fX,Pk,Hk,Rk,Z(k,:
)'-hfX);
X_est(k,:
)=Xk';
end
figure
(1),plot(X_est(:
1),X_est(:
3),'+r')
xlabel('X');ylabel('Y');title('ekfsimulation');
legend('real','measurement','ekfestimated');
%%%%%%%%%%%%%%%%%%%%子程序%%%%%%%%%%%%%%%%%%%
functionF=JacobianF(X,kx,ky,g)%系统状态雅可比函数
vx=X
(2);vy=X(4);
F=zeros(4,4);
F(1,2)=1;
F(2,2)=-2*kx*vx;
F(3,4)=1;
F(4,4)=2*ky*vy;
functionH=JacobianH(X)%量测雅可比函数
x=X
(1);y=X(3);
H=zeros(2,4);
r=sqrt(x^2+y^2);
H(1,1)=1/r;H(1,3)=1/r;
xy2=1+(x/y)^2;
H(2,1)=1/xy2*1/y;H(2,3)=1/xy2*x*(-1/y^2);
functionfX=fff(X,kx,ky,g,Ts)%系统状态非线性函数
x=X
(1);vx=X
(2);y=X(3);vy=X(4);
x1=x+vx*Ts;
vx1=vx+(-kx*vx^2)*Ts;
y1=y+vy*Ts;
vy1=vy+(ky*vy^2-g)*Ts;
fX=[x1;vx1;y1;vy1];
functionhfX=hhh(fX,Ts)%量测非线性函数
x=fX
(1);y=fX(3);
r=sqrt(x^2+y^2);
a=atan(x/y);
hfX=[r;a];
function[Xk,Pk,Kk]=ekf(Phikk_1,Qk,fXk_1,Pk_1,Hk,Rk,Zk_hfX)%ekf滤波函数
Pkk_1=Phikk_1*Pk_1*Phikk_1'+Qk;
Pxz=Pkk_1*Hk';Pzz=Hk*Pxz+Rk;Kk=Pxz*Pzz^-1;
Xk=fXk_1+Kk*Zk_hfX;
Pk=Pkk_1-Kk*Pzz*Kk';
图2仿真结果
欢迎您的下载,
资料仅供参考!
致力为企业和个人提供合同协议,策划案计划书,学习资料等等
打造全网一站式需求
4欢迎下载
。
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 扩展 卡尔 滤波 EKF 仿真 演示