kalman滤波预测速度之只有位置观测量的小汽车匀速直线运动模型

clear all;
R=2;
N=1000;
v=normrnd(0,sqrt(R),[1,N]);
k=[0:N-1];
dt=0.1;
vel_theory=10;%理论速度
pos_theory=12+k*vel_theory*dt;
pos_ob=pos_theory+v;
vel_pred=linspace(0,0,N);%预测的速度
pos_pred=linspace(0,0,N);
A=[1 dt;0 1];
H=[1 0];
xk=[0 0]';
P=[0.01 0.01;0.01 0.001];
Q=[0.01 0;0 0.01];
for i=1:1:N
    xk=A*xk;
    P=A*P*A'+Q;
    Kg=P*H'/(H*P*H'+R);
    xk=xk+Kg*(pos_ob(i)-H*xk);
    pos_pred(i)=xk(1);
    vel_pred(i)=xk(2);
end
plot(vel_pred,'r')