【卡尔曼滤波跟踪】跟踪目标的轨迹，并将滤波器输出与原始轨迹进行比较（Matlab实现）

荧光Matlab

已于 2024-05-10 16:15:18 修改

阅读量267

点赞数 4

文章标签： matlab 开发语言

于 2024-05-10 15:56:23 首次发布

本文链接：https://blog.csdn.net/Matlab88888/article/details/138669599

版权

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

💥1 概述

使用卡尔曼滤波器（constant acceleration）跟踪目标轨迹是一种常见的目标跟踪方法卡尔曼滤波器是一种递归状态估计算法，用于从一系列不完全和包含噪声的测量中，估计动态系统的状态。在跟踪目标轨迹时，卡尔曼滤波器通过预测目标的位置和速度，并将测量值与预测值进行比较，实现对目标位置的优化估计。卡尔曼滤波器广泛应用于目标跟踪、导航、控制等领域。在目标跟踪中，卡尔曼滤波器可以用于跟踪各种类型的目标，如飞行器、车辆、行人等，在不同的传感器数据下实现准确的轨迹跟踪。基于卡尔曼滤波器的目标轨迹跟踪利用动态系统的状态估计和测量值的融合，实现对目标轨迹的准确跟踪，具有广泛的应用价值和研究意义。

📚2 运行结果

主函数部分代码：

% Program with state vector containing acceleration term			
clc;
clear all;
close all;
int_dis=5000;
v=300;
theta1= pi/4;
g=9.8;
f=3;      % acceleration factor
ann=0.005; % noise angle normalization 
rnn=30;   % noise range normalization
 
% 1st trajectory
t= [0:0.5:50];
r1= (int_dis +v*t);
x1= r1.* cos(theta1);
y1= r1.* sin(theta1);
figure(1);
plot(x1,y1, '.-b');
hold on;
figure(2);
plot(x1,y1, 'b');
rmax=(int_dis+v*50);
xmax=rmax*cos(theta1);
ymax=rmax*sin(theta1);
hold on;
grid on;
 
% Adding noise to first trajectory
t= [0:0.5:49.5];
r1= (int_dis+v.*t);
nr1=rnn*randn(size(t));
rn1=r1+nr1;
nt1=ann*randn(size(t));
thetan1=theta1+nt1;
xn1= rn1.* cos(thetan1);
yn1= rn1.* sin(thetan1);
figure(2);
plot(xn1,yn1, '.-r');
figure(3);
plot(xn1,yn1, '.-r');
hold on;
 
 %2nd trajectory
rr2=(v^2/(f*g));
phi1= atan(rr2/(rmax-int_dis));
phi2=atan(rr2/(rmax-int_dis));
phi=(phi1+phi2);
t2= (pi+phi)*rr2./(300);
t=[50:0.5:50+t2];
thetar2= linspace(3*pi/4,-theta1-phi,2*t2+1);
xc= xmax+rr2*cos(theta1);
yc= ymax-rr2*sin(theta1);
x2=rr2.*cos(thetar2)+xc;
y2=rr2.*sin(thetar2)+yc;
figure(1);
plot(x2,y2,'.-b');
grid on;
title('True trajectory');
figure(2);
plot(x2,y2,'b');
title('True trajectory & Noise');
axis equal;
r2=sqrt(x2.^2+y2.^2);
theta2= atan(y2./x2);