使用matlab对输入数据进行卡尔曼滤波

使用matlab对输入数据进行卡尔曼滤波。

简单说明一下程序中的数据。假设一辆汽车从初始点（0，10）开始行驶，初始速度沿y轴正方向10m/s。然后在观测途中向右先加速再减速变换车道。

整个过程其实有x轴坐标，y轴坐标，x轴速度，y轴速度以及x轴的加速度这5个物理量。但在卡尔曼滤波时我只用了x轴坐标，y轴坐标，x轴速度，y轴速度这4个量，相当于将x轴的人为施加的加速度当作了噪声处理。人为施加的加速度当然不符合高斯分布，有违卡尔曼滤波的假设，这里只是为现实应用直接处理的。有兴趣的可以把x轴的加速度这个量也纳入到卡尔曼滤波中。

clc
clear
close all
%% 汽车变道数据生成
% 汽车初始位置为（0，10）,初始x方向速度为0，y方向速度为5m/s，向前匀速运动
% 在第400-600帧时，向右加速，加速度为0.05m/s^2
% 在第601-610帧时，停止加速
% 在第611-810帧时，向左加速，加速度为0.05m/s^2，正好使得x轴方向速度为0，完成变道

%间隔时间步长.每秒30帧
T = 1/30;
%总的观测帧数目
totaltime = 1600;                             
% 状态转移矩阵F
f_1 = [1 T T^2/2;0 1 T;0 0 1];
f_2 = [1 T;0 1];
F = blkdiag(f_1,f_2);
% [初始x坐标， 初始x速度， 初始x加速度， 初始y坐标， 初始y速度]
X(:,1) = [0 0 0 10 5]';
% 观测矩阵H，观测到x坐标，x速度， y坐标， y速度四个量
H = [1 0 0 0 0;0 1 0 0 0;0 0 0 1 0; 0 0 0 0 1];
% 过程噪声，可能路面比较滑，风力等影响，对下个时刻的真实状态造成影响
sigma = 0.00005;
Q = diag([sigma sigma/T 0 sigma sigma/T])