第06章Kalman滤波解说.ppt

则由(6.3.2)和(6.3.5)有新息表达式 由 得 为求Kalman滤波器增益 ，为此，求 用 简化 ，得： 定理1 系统(6.3.1)和(6.3.2)在假设6.3.1、假设6.3.2下，递 推Kalman滤波器为 在定理1条件下，Kalman滤波器有另一种递推形式 初值取(6.3.7)是为了保证估值的无偏性。Kalman滤波器计算程序框图如图1所示。 图1 Kalman滤波计算法程序框图 clear %引入一个离散控制过程的系统(线性随机微分方程) %X(k)=A X(k-1)+B U(k)+W(k) N=10000;Value=25;W=randn(1,N); %带高斯噪声（过程噪声）的信号，图中为蓝线 S=Value+W; %卡尔曼初始估计值（任意取值，P(1)不为零，为零时最优化） X(1)=20;P(1)=8; %均值为H*Value的高斯噪声（测量噪声）的观测变量 V=randn(1,N);H=0.2;Z=H*Value+V; %W(k)和V(k)分别表示过程和测量的噪声,假设成高斯白噪声,协方差分别是Q，R R=std(V).^2;Q=1e-6; %Q假设常数,若增加过程噪声，则Q=std(W).^2; %实际测量值，偏差是均值为H*Value的高斯噪声，图中为黄线 Y=Value+Z; %卡尔曼估计值，图中为红线 A=1;%假设Value是跟前一时刻的Value相同，所以A=1 B=1;U(1)=0;precision=0.0001;%状态控制量,微调精度 for k=1:N-1 %时间更新（预测） X(k+1)=A*X(k)+B*U(k); %向前推算状态变量 %X(k+1)=A*X(k)+B*U(k)+W(k); %增加过程噪声 %***(1)X(k|k-1)=A X(k-1|k-1)+B U(k) %*** A和B是系统参数 %*** X(k|k-1)是利用上一状态预测的结果 %*** X(k-1|k-1)是上一状态最优的结果 %*** U(k)为现在状态的控制量,如果没有控制量，它可以为0 %*** W(k)和V(k)分别表示过程和测量的噪声 P(k+1)=A*P(k)*A+Q; %向前推算误差协方差 %***(2)P(k|k-1)=A P(k-1|k-1)A’+Q %*** P(k|k-1)是X(k|k-1)对应的协方差 %*** P(k-1|k-1)是X(k-1|k-1)对应的协方差 %*** A’表示 A的转置矩阵 %*** Q是系统过程的协方差 %测量更新（校正） Kg(k)=P(k+1)*H*inv(H*P(k+1)*H+R); %计算卡尔曼增益 %***(4)Kg(k)=P(k|k-1)H’/(H P(k|k-1)H’+R) %*** 结合预测值和测量值，可以得到现在状态(k)的最优化估算值X(k|k) X(k+1)=X(k+1)+Kg(k)*(Z(k)-H*X(k+1)); %由观测变量Z(k)更新估计 %***(3)X(k|k)= X(k|k-1)+Kg(k)(Z(k)-H X(k|k-1)) %*** 现在状态(k)的最优化估算值X(k|k) %*** 其中Kg为卡尔曼增益(Kalman Gain) P(k+1)=P(k+1)-Kg(k)*H*P(k+1); %更新误差协方差 %***(5)P(k|k)=（I-Kg(k) H）P(k|k-1) %*** I为1的矩阵，对于单模型单测量，I=1 %*** 当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1) %*** 算法进行自回归运算 %状态控制微调 if X(k+1)X(k) U(k+1)=-precision;end if X(k+1)==X(k) U(k+1)=0;end if X(k+1)X(k) U(k+1)=precision;end End %图形表示 t=1:N;plot(t,S,b,t,Y,y,t,Value,g,t,X,r); %带高斯噪声（过程噪声）的信号，图中为蓝线 %实际测量值，偏差是均值为H*Value的高斯噪声，图中为黄线 %标准值，