卡尔曼滤波动态跟踪.rar

在做课程设计的时候遇到了运用卡尔曼滤波进行移动目标跟踪问题，对此参考了很多资源，完成了题目要求，如果有做卡尔曼滤波的可以放心下载，有详尽注释。

clc;clear all;close all;
T=1;  %采样周期
N=100/T;  %总的采样次数
r=1.01;var_u=1;var_w=3;   %几个参数初始化赋值
A=[r000;0r00;T*（1+r）/2010;0T*（1+r）/201];
B=[10T/20;010T/2]‘;
Q=var_u*eye（2）;
H=[0010;0001];
p11=（T^2*var_u+8*r^2*var_w）/（T^2*（1+r）^2）;
p13=2*r*var_w/（T*（1+r））;
M22=[p110p130;0p110p13;p130var_w0;0p130var_w];

xd=zeros（2N）;
S=zeros（4N）;  %信号矢量vxvyxy
S（:1）=[1111]; %x四维，初始绝对位置（11）水平速度2，垂直速度2   %（:1）取第一列
Z=zeros（2N）;  
Z（:1）=[S（31）S（41）];  %z只观测到位置，没有速度，zx观测值
Z1=zeros（2N）;           %z真实相对观测值
Z1（:1）=Z（:1）;  %z只观测到位置，没有速度，zx观测值
Skf=zeros（4N）;  %卡尔曼滤波结果
Skf（:1）=S（:1）; 
%%%运动模型，状态模型s（n）=As（n-1）+Bn和观测到的Z
for i = 2:N
    S（:i）=A*S（:i-1）+B*var_u*randn（21）;
    Z（:i）=H*S（:i）+var_w*randn（21）;
    xd（:i）=xd（:i-1）+[1;0];       %沿着x轴匀速    
    Z1（:i）=Z（:i）-xd（:i）;    %观测真实相对值

end
zx1=Z（11）;zx2=Z（12）;zy1=Z（21）;zy2=Z（22）;
db=2*r/（T*（1+r））;
Skf（:2）=[db*（zx2-zx1）db*（zy2-zy1）zx2zy2];%s[2|2]
%%%滤波的
for i=3:N  
    S_pre=A*Skf（:i-1）;                           %s[k|k-1]=As[k-1|k-1]
    M_pre=A*M22*A‘+var_w*（B*B‘）;                  %M[k|k-1]=AM[k-1|k-1]A‘+var_w*BB‘
    K=M_pre*H‘/（H*M_pre*H‘+var_w*eye（2））;         %K[k]=M[k|k-1]H‘（HM[k|k-1]H‘+var_w*I）-1
    Skf（:i）=S_pre+K*（Z（:i）-H*S_pre）;  
    M22=（eye（4）-K*H）*M_pre;                      %M[k|k]=（I-KH）*M[k|k-1]
end  

%%%绘图
figure;
hold on;
plot（S（3:）S（4:）‘-k‘）;  %位置轨迹
plot（Z（1:）Z（2:）‘-b.‘）;  %观测轨迹
% plot（Z1（1:）Z1（2:）‘-m*‘）; %真实位置
plot（Skf（3:）Skf（4:）‘-r+‘）;  %Kalman估计轨迹
hold off;legend（‘真实轨迹‘‘观测轨迹‘‘滤波轨迹‘）  
%%%误差计算模块
for i=1:N  
    Err_Observation（i）=RMS（S（:i）Z（:i））;  
    Err_KalmanFilter（i）=RMS（Skf（:i）Z（:i））;  
end 
figure;  
hold on; box on;  
plot（Err_Observation‘-ko‘‘MarkerFace‘‘g‘）  
plot（Err_KalmanFilter‘-ks‘‘MarkerFace‘‘r‘）  
legend（‘滤波前误差‘‘滤波后误差‘） 

%%%第二问引入传感器误差引入改变之后的矩阵
% var_d=2;      %传感器方差
% var_n=var_d+var_w;%M[2|2]中出现的
% p11_n=（T^2*var_u+8*r^2*var_n）/（T^2*（1+r）^2）;
% p13_n=2*r*var_n/（T*（1+r））;
% M22_n=[p11_n0p13_n0;0p11_n0p13_n;p13_n0var_n0;0p13_n0var_n];
% Skf_n=zeros（4N）;  %卡尔曼滤波结果
% Skf_n（:1）=S（:1）; 

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       2372  2020-04-12 19:33  卡尔曼滤波\kal_test.m

     文件       2376  2020-04-12 19:36  卡尔曼滤波\kal_test2.m

     文件        174  2020-04-12 10:52  卡尔曼滤波\RMS.m

     文件     149886  2020-04-14 23:16  卡尔曼滤波\题目描述1.png

     文件      41930  2020-04-14 23:17  卡尔曼滤波\题目描述2.png

     目录          0  2020-04-14 23:18  卡尔曼滤波

----------- ---------  ---------- -----  ----

               196738                    6