close all;
clear all;
clc;
data = importdata('Data.mat');
t = data.t;
x0 = data.x;
y0 = data.y;
position = [x0' y0'];
figure
plot(position(:, 1), position(:, 2),'*');
title('滤波前轨迹');
grid on;
hold on;
T = 1;
Kalmanfilter_S = zeros(size(position, 1), 4);
for i = 1 : length(t)
if i == 1
K = 0; %状态滤波增益矩阵
x = 0; %观测信号
M = zeros(4, 4); %初始M设为0
s = [position(i, 1); position(i, 2); 0; 0]; %由题目可知,系统状态s为[x, y,vx,vy], 其中(x,y)分别为位置坐标,vx vy分别为沿x轴和y轴的速度分量
A = [1 0 T 0;0 1 0 T;0 0 1 0;0 0 0 1]; %M维状态转移矩阵
Cw =[0.001 0 0 0;0 0.001 0 0;0 0 0.001 0;0 0 0 0.001]; %观测噪声的协方差矩阵
H = [1 0 0 0;0 1 0 0]; %观测矩阵
Cn = [3 0;0 3]; %预测噪声的协方差矩阵
B = [1 0 0 0;0 2 0 0;0 0 1 0;0 0 0 1];
else
x = position(i, 1:2)';
sk = A * s;
Mk = A * M * A' + B*Cw*B';
K = Mk * H' * (H * Mk * H' + Cn)^-1;
M = Mk - K * H * Mk;
s = sk + K * (x - H * sk);
% sk = A * s;
% Mk = A * M * A' + Cw;
% K = Mk * H' * (H * Mk * H' + Cn)^-1;
% s = sk + K * (x - H * sk);
% M = Mk - K * H * Mk;
end
Kalmanfilter_S(i, :) = s';
end
result = Kalmanfilter_S(:, 1:2);
plot(result(:, 1), result(:, 2));
title('卡尔曼滤波后的结果');
xlabel('X轴方向距离/m');
ylabel('Y轴方向距离/m');
legend('滤波器前点迹','滤波后轨迹');
grid on;
fprintf('X轴方向速度为:%fm/min\n',abs(Kalmanfilter_S(52,3)));
fprintf('Y轴方向速度为:%fm/min\n',abs(Kalmanfilter_S(52,4)));
fprintf('该学生的平均跑步速度为:%fm/min\n',sqrt((sum(Kalmanfilter_S(:,3)/52).^2+(sum(Kalmanfilter_S(:,4)/52).^2))));