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:Nxk=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);endplot(vel_pred,'r')