【卡尔曼滤波器、IMM】基于matlab编程二维空间内目标作匀速直线运动和匀速圆周运动的特点
2023-09-11 14:15:33 时间
1.软件版本
matlab2021a
2.本算法理论知识
3.核心代码
根据二维空间内目标作匀速直线运动和匀速圆周运动的特点,在建立目标运动模型和观测模型的基础上采用基于交互多模算法(IMM)的卡尔曼滤波器对机动目标进行跟踪。仿真结果表明,该算法不仅能够对匀速直线运动和匀速圆周运动的目标进行跟踪,而且在运动模型发生变化时,滤波误差也比较小。 卡尔曼滤波器;目标跟踪;机动;交互多模(IMM)
T=2;
alpha=0.8; % 加权衰减因子
window=1/(1-alpha); % 检测机动的有效窗口长度
dt=100; % dt=dt_x=dt_y=100
Th=25; % 机动检测门限
Ta=9.49; % 退出机动的检测门限
N=800/T; % 采样次数
M=50; % 模拟次数
% 真实轨迹数据
t=2:2:400;
xo0=2000+0*t;
yo0=10000-15*t;
t=402:2:600;
xo1=2000+0.075*((t-400).^2)/2;
yo1=10000-15*400-(15*(t-400)-0.075*((t-400).^2)/2);
t=602:2:610 ;
xo2=xo1(100)+15*(t-600);
yo2=yo1(100)+0*t;
t=612:2:660;
xo3=xo2(5)+(15*(t-610)-0.3*((t-610).^2)/2);
yo3=yo2(5)-0.3*((t-610).^2)/2;
t=662:2:800;
xo4=xo3(25)+0*t;
yo4=yo3(25)-15*(t-660);
x=[xo0,xo1,xo2,xo3,xo4];
y=[yo0,yo1,yo2,yo3,yo4];
e_x1=zeros(1,N);
e_x2=zeros(1,N);
e_y1=zeros(1,N);
e_y2=zeros(1,N);
px=zeros(1,N);
qy=zeros(1,N);
u=zeros(1,N);
u_a=zeros(1,N);
for j=1:M
no1=100*randn(1,N); % 随机白噪
no2=100*randn(1,N);
for i=1:N;
zx(i)=x(i)+no1(i); % 观测数据
zy(i)=y(i)+no2(i);
z(:,i)=[zx(i);zy(i)];
end
%
X_estimate(2,:)=[zx(2),(zx(2)-zx(1))/T,zy(2),(zy(2)-zy(1))/T];
X_est=X_estimate(2,:);
P_estimate=[dt^2,dt^2/T,0,0;dt^2/T,(dt^2)*2/(T^2),0,0;0,0,dt^2,dt^2/T;0,0,dt^2/T,(dt^2)*2/(T^2)];
x1(1)=zx(1); y1(1)=zy(1); x1(2)=X_estimate(2,1); y1(2)=X_estimate(2,3);
u(1)=0; u(2)=0;
u1=u(2);
pp=0;% 0表示非机动,1表示机动
qq=0;
rr=1;
k=3;
while k<=N
if k<=20
z1=z(:,k);
[X_pre,X_est,P_estimate,u1]=kalmanstatic(X_est,P_estimate,z1,k,u1);
X_estimate(k,:)=X_est;
X_predict(k,:)=X_pre;
P(k,:)=[P_estimate(1,1),P_estimate(1,2),P_estimate(2,2),P_estimate(3,3),P_estimate(3,4),P_estimate(4,4)];
x1(k)=X_estimate(k,1);
y1(k)=X_estimate(k,3);
u(k)=u1;
k=k+1;
else
if pp==0 % 进入非机动模型
if rr==window+1 % 由机动进入非机动模型,为防止回朔,至少递推window+1次
u1=0;
else
end
while rr>0
z1=z(:,k);
[X_pre,X_est,P_estimate,u1]=kalmanstatic(X_est,P_estimate,z1,k,u1);
X_estimate(k,:)=X_est;
X_predict(k,:)=X_pre;
P(k,:)=[P_estimate(1,1),P_estimate(1,2),P_estimate(2,2),P_estimate(3,3),P_estimate(3,4),P_estimate(4,4)];
x1(k)=X_estimate(k,1);
y1(k)=X_estimate(k,3);
u(k)=u1;
rr=rr-1;
end
rr=1;
if u(k)>=Th
pp=1;qq=1; % “pp=1,qq=1”表示由非机动进入机动模型
else
end
k=k+1;
else % 机动模型
if qq==1 %由非机动进入机动模型,需要进行修正
k=k-window-1;
Xm_est=[X_estimate(k-1,:),0,0];
Xm_pre=[X_predict(k,:),0,0];
Pm_estimate=zeros(6,6);
P=P(k-1,:);
m=0;
else %在机动模型中进行递推
Xm_est=Xm_estimate(k-1,:);
end
z1=z(:,k);
[Xm_est,Pm_estimate,ua1,qq,m]=kalmandynamic(Xm_pre,Xm_est,Pm_estimate,z1,k,P,qq,m);
Xm_estimate(k,:)=Xm_est;
x1(k)=Xm_estimate(k,1);
y1(k)=Xm_estimate(k,3);
ua(k)=ua1;
if ua1<Ta % 进入非机动模型,降维,标志 pp=0
X_est=Xm_estimate(k,1:4);
P_estimate=Pm_estimate(1:4,1:4);
pp=0;
rr=window+1;
else
end
k=k+1;
end
end
end
for j=1:N
px(1,j)=x1(1,j)+px(1,j); % 迭加每次估计的数据
qy(1,j)=y1(1,j)+qy(1,j);
e_x1(j)=(x1(j)-x(j))+e_x1(j);
e_y1(j)=(y1(j)-y(j))+e_y1(j);
e_x2(j)=((x1(j)-x(j))^2)+e_x2(j);
e_y2(j)=((y1(j)-y(j))^2)+e_y2(j);
end
end
for k=1:N
px(1,k)=px(1,k)/M;
qy(1,k)=qy(1,k)/M;
e_x(k)=e_x1(k)/M;
ex(k)=sqrt(e_x2(k)/M-e_x(k)^2);
e_y(k)=e_y1(k)/M;
ey(k)=sqrt(e_y2(k)/M-e_y(k)^2);
end
figure(1);
plot(x,y);axis([1500 5000 -2000 12000]);
figure(2);
plot(x,y,'b-',zx,zy,'k:',px,qy,'r');
legend('真实轨迹','观测轨迹','50次滤波轨迹');
figure(3);
plot(x,y,'k',x1,y1,'r');
legend('真实轨迹','一次滤波轨迹');
figure(4);
subplot(2,2,1),plot(e_x); title('X坐标 滤波误差均值曲线');
subplot(2,2,2),plot(e_y); title('Y坐标 滤波误差均值曲线');
subplot(2,2,3),plot(ex); title('X坐标 滤波误差标准差曲线');
subplot(2,2,4),plot(ey); title('Y坐标 滤波误差标准差曲线');
4.操作步骤与仿真结论
5.参考文献
[1] Wu W , Cao P , Pan Z D . Maneuver Target Tracking Based on Kalman IMM Algorithm[J]. Springer Berlin Heidelberg, 2012.
D211
6.完整源码获得方式
方式1:微信或者QQ联系博主
方式2:订阅MATLAB/FPGA教程,免费获得教程案例以及任意2份完整源码
相关文章
- Matlab中fclose函数使用
- 【火灾疏散建模】基于MATLAB的火灾人员疏散建模编程和仿真分析
- 【MATLAB教程案例97】基于GA遗传优化的CNN卷积神经网络最优训练参数搜索matlab仿真
- 【MATLAB教程案例94】基于Matlab的IHS图像融合算法仿真案例
- 【MATLAB教程案例88】通过matlab模拟光的干涉现象
- 【MATLAB教程案例79】基于移动节点WSN的最短路由matlab仿真——应用于车组网或无人机组网等
- 【MATLAB教程案例76】基于dijkstra算法的WSN最短路由matlab仿真
- 【MATLAB教程案例66】基于Q-learning强化学习的智能体栅格地图路线规划matlab仿真
- 【MATLAB教程案例62】使用matlab实现基于PointNet++网络的点云数据分类仿真分析
- 【MATLAB教程案例60】使用matlab实现基于GRU网络的数据分类预测功能与仿真分析
- 【MATLAB教程案例50】通过VisualSFM工具箱提取360度等间隔环绕拍摄得到的图像序列点云数据,并进行目标三维重建matlab仿真
- 【MATLAB教程案例27】基于matlab的图像配准算法的仿真与分析——sift,surf,kaze等
- 【MATLAB教程案例17】基于NSGAII多目标优化算法的matlab仿真及应用
- 【MATLAB教程案例15】基于WOA鲸鱼优化算法的函数极值计算matlab仿真及其他应用
- 【MATLAB教程案例14】基于ACO蚁群优化算法的函数极值计算matlab仿真及其他应用
- 【MATLAB教程案例11】基于PSO粒子群优化算法的函数极值计算matlab仿真及其他应用
- 【MATLAB教程案例6】基于Costas环的载波同步matlab仿真
- costas载波同步matlab编程源码
- 【Matlab算法】修正G-N法求解非线性最小二乘优化问题(附修正G-N法MATLAB代码)