小车直线运动卡尔曼滤波，入门简单程序，拿走不谢。

代码来自于B站一个视频，我把它写出来并且在于变量noise我觉得他写的不够好所以自作主张改进一下，自己也补充了一点注释，增加了迭代次数，等小改动，欢迎批评指正。多谢。
该视频在B站的地址：https://www.bilibili.com/video/av4356232?from=search&seid=16010925919274859096

z = (1:300);%观测值矩阵
noise = 1+ sqrt(1) * randn(1,300);%方差为1的高斯噪声，很大了
z=z+noise;%带噪声的观测值
X = [0;0];%初始状态，第一个是位置，第二个是速度，初始值不是很重要
P = [1 0; 0 1];%初始的协方差矩阵，初始值不是很重要
F = [1 1;0 1];%转态转移矩阵，第一行第二列是时间步长，看看小车例子就知道这个转移矩阵怎么来
Q = [0.001 0;0 0.001];%转态转移矩阵的协方差
H = [1 0];%观测矩阵
R =1;%观测噪声,噪声方差
figure;
hold on;
for i=1:300%迭代300次，直接带入卡尔曼滤波的五条公式，（注意下一行开始到end行之前，在matlab中要缩进，自己补充上）
X_ = FX;
P_ = FPF’+Q;
K = P_H’/(HP_H’+R);
X = X_+K(z(i) - HX_);
P = (eye(2)-K*H)*P_;
plot(X(1),X(2),’.’,‘Color’,[1 0 0]);%第一个（横轴）表示位置，第二（纵轴）表示速度
axis([0 300,0 2])；
end