适合卡尔曼滤波的初学者，仿真了匀速运动状态下，卡尔曼稳定跟踪物体轨迹的代码

适合卡尔曼滤波的初学者，仿真了匀速运动状态下，卡尔曼稳定跟踪物体轨迹的代码

%---------------------------------------------%
%					      %
%          工作室提供代做matlab仿真	      %
%					      %
%  详情请访问：http://cn.mikecrm.com/5k6v1DP  %
%					      %
%---------------------------------------------%

%======卡尔曼算法，状态参数有两个，速度与位置，估计其值======
%=======对匀速运动物体的跟踪：速度和位移================

%%
clc ;
clear;
n=100;                  %100个点
Yk          =zeros（2n）;
Xk_real     =zeros（2n）;
Xk_real（:1） =[5015]‘;%position and speed
Z（:1）=50;%观测值初始化
F=[11;01];%状态转移矩阵2*2
H=[1 0];%观测矩阵1*2
R=1;
% Q=1e-3;%经验取值
Xk（:1）=[3;1];
P0=eye（2）;

%观测值与状态值生成
for i=2:n
    Xk_real（:i）=F*Xk_real（:i-1）;%+sqrt（Q）*randn（21）;%2*2与2*1=2*1
    Yk（i）=H*Xk_real（:i） +sqrt（R）*randn（1）;
end

%卡尔曼迭代
for i=2:n
    Xn     =F*Xk（:i-1）;         %2*1
    Pk     =F*P0*F‘;           %2*2
    K      =Pk*H‘*inv（R+H*Pk*H‘）;   %2*1
    Xk（:i）=Xn+K*（Yk（i）-H*Xn）;       %2*1
    P0     =（eye（2）-K*H）*Pk;
end

%速度误差与位移误差
error_p=Xk_real（1:）-Xk（1:）;
error_s=Xk_real（2:）-Xk（2:）;
%%
figure;
plot（Xk_real（1:）‘r-‘）;
hold on
plot（Xk（1:）‘y*‘）plot（error_p）;
grid on
xlabel（‘间隔‘）;
ylabel（‘位置‘）;
legend（‘真实位置轨迹‘‘滤波位置轨迹‘‘误差‘）;
title（‘真实轨迹与滤波轨迹‘）;

figure（）;
plot（Xk_real（2:）‘y-‘）;
hold on
plot（Xk（2:）‘g*-‘）plot（error_s‘r*-‘）;
grid on
xlabel（‘间隔‘）;
ylabel（‘速度‘）;
legend（‘真实速度轨迹‘‘滤波速度轨迹‘‘误差‘）;
title（‘真实轨迹与滤波轨迹‘）;

    

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

     文件       1522  2018-09-19 08:09  适合卡尔曼滤波的初学者，仿真了匀速运动状态下，卡尔曼稳定跟踪物体轨迹的代码\a.m

     文件         55  2018-08-27 11:33  适合卡尔曼滤波的初学者，仿真了匀速运动状态下，卡尔曼稳定跟踪物体轨迹的代码\【源码使用必读】.url

     目录          0  2018-09-16 11:01  适合卡尔曼滤波的初学者，仿真了匀速运动状态下，卡尔曼稳定跟踪物体轨迹的代码

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

                 1577                    3