%---------------------------------------------%
% %
% 工作室提供代做matlab仿真 %
% %
% 详情请访问：http://cn.mikecrm.com/5k6v1DP %
% %
%---------------------------------------------%
%======卡尔曼算法，状态参数有两个，速度与位置，估计其值======
%=======对匀速运动物体的跟踪：速度和位移================
%%
clc ;
clear;
n=100; %100个点
Yk =zeros(2,n);
Xk_real =zeros(2,n);
Xk_real(:,1) =[50,15]';%position and speed
Z(:,1)=50;%观测值初始化
F=[1,1;0,1];%状态转移矩阵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(2,1);%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('真实轨迹与滤波轨迹');
