经典卡尔曼滤波 目标跟踪 matlab 程序注释详细 新手入门

经典卡尔曼滤波 目标跟踪 程序注释详细 新手入门 matlab 程序无bug 完美运行 重要的事情说三遍！包教包会 包教包会 包教包会！

%xy轴方向——二维估计
clc
close all
clear all

%参数
T=1;                                  %采样间隔
q1=1;                                 %x过程噪声方差E[v^2（k）=q1]
q2=1;                                 %y过程噪声方差E[v^2（k）=q2]
r=1;                                  %量测噪声方差E[W^2（k）=r=1]
F=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];  %状态转移矩阵
C=[T^2/2 0;T 0;0 T^2/2;0 T];          %过程噪声分布矩阵
H=[1 0 0 0;0 0 1 0];                  %量测矩阵

%初始化
X（:1）=[10;2;10;2];           %目标真实初始状态
W=normrnd（0r21）;             %量测噪声
Z（:1）=H*X（:1）+W;              %量测方程Z（k）=H（k）*X（k）+W（k）
v1=normrnd（0q111）;           %x过程噪声
v2=normrnd（0q211）;           %y过程噪声
X（:2）=F*X（:1）+C*[v1;v2];      %状态方程X（k+1）=F（k）*X（k）+C（k）*v（k）
W=normrnd（0r21）;
Z（:2）=H*X（:2）+W;

%四维状态向量估计的初始化
XX（:1）=[Z（12）;（Z（12）-Z（11））/T;Z（22）;（Z（22）-Z（21））/T];        %初始状态估计
theta=atan（（Z（22）-Z（21））/（Z（12）-Z（11）））;
L=sqrt（（Z（22）-Z（21））^2+（Z（12）-Z（11））^2）;
A=[cos（theta） -L*sin（theta）;sin（theta） L*cos（theta）];
R=A*[2 0;0 pi/180]*A‘;                                              %径向距离方差为2，方位角测量误差方差为1°
P（:1:4）=[R（11）      R（11）/T       R（12）       R（12）/T
          R（11）/T    2*R（11）/（T^2） R（12）/T     2*R（12）/（T^2）
          R（12）      R（12）/T       R（22）       R（22）/T
          R（12）/T    2*R（12）/（T^2） R（22）/T     2*R（22）/（T^2）];  %初始协方差

%卡尔曼滤波      
for k=1:999
    
    XX1（:k+1）=F*XX（:k）;               %状态的一步预测XX1（k+1|k）=F*XX（k|k）+G*u
    ZZ（:k+1）=H*XX1（:k+1）;             %量测的预测ZZ（k+1|k）=H*XX1（k+1|k）
    
    v1=normrnd（0q111）;               %x过程噪声
    v2=normrnd（0q211）;               %y过程噪声
    X（:k+2）=F*X（:k+1）+C*[v1;v2];      %状态方程
    W=normrnd（0r21）;    
    Z（:k+2）=H*X（:k+2）+W;              %量测方程
    U（:k+1）=Z（:k+2）-ZZ（:k+1）;        %新息U（k）=Z（k+1）-ZZ（k+1|k）
    
    PP（:4*k+1:4*k+4）=F*P（:4*k-3:4*k）*F‘+C*[q1 0;0 q2]*C‘;   %协方差的一步预测PP（k+1|k）=F*P（k|k）*F‘+Q（k）协方差定值
    theta=atan（（Z（2k+2）-Z（2k+1））/（Z（1k+2）-Z（1k+1）））;
    L=sqrt（（Z（2k+2）-Z（2k+1））^2+（Z（1k+2）-Z（1k+1））^2）;
    A=[cos（theta） -L*sin（theta）;sin（theta） L*cos（theta）];
    R=A*[2 0;0 3]*A‘;                                         %k时刻量测噪声协方差
    S（:2*k:2*k+1）=H*PP（:4*k+1:4*k+4）*H‘+R;%[r 0;0 r];       %新息协方差S（k+1）=H*PP（k+1|k）*H‘+R（k+1），协方差定值
 
    
    K（:2*k:2*k+1）=PP（:4*k+1:4*k+4）*H‘/S（:2*k:2*k+1）;       %增益K（k+1）=PP（k+1|k）*H‘/S（k+1）
    
    XX（:k+1）=XX1（:k+1）+K（:2*k:2*k+1）*U（:k+1）;             %状态更新方程XX（k+1|k+1）=XX1（k+1|k）+K（k+1）*U（k+1）
    P（:4*k+1:4*k+4）=PP（:4*k+1:4*k+4）-K（:2*k:2*k+1）*S（:2*k:2*k+1）*K（:2*k:2*k+1）‘; %协方差更新方程P（k+1|k+1）=PP（k+1|k）-K*S*K‘
     
end

for i=1:999
    D（1i）=PP（14*i-3）;    %X预测位置
    D（2i）=P（14*i-3）;     %X更新位置
    D（3i）=PP（34*i-1）;    %Y预测位置
    D（4i）=P（34*i-1）;     %Y更新位置
    V（1i）=PP（24*i-2）;    %X预测速度
    V（2i）=P（24*i-2）;     %X更新速度
    V（3i）=PP（44*i）;      %Y预测速度
    V（4i）=P（44*i）;       %Y更新速度
end

for i=2:999
    DD（1i）=sqrt（（ D（1i）^2）*（ D（3i）^2））;                 

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

     文件       4964  2017-03-01 08:25  kalman.m

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

                 4964                    1