递归最现代灯泡乘估计Matlab模拟递归最现代灯泡乘估计理论模拟背景Matlab程序模拟动态图
递归最现代灯泡乘估计理论
递推最现代的灯泡乘是卡尔曼滤波的重要基础，关于最现代的灯泡乘理论请参考以前的文章《最现代的灯泡乘估计》。 这里只给出了递推最现代的灯泡乘方估算步骤
计算增益
k=pk1孜孜叶片(hkpk1孜孜叶片R k ) 1k_k=p_{k-1}h_k^t ) h_KP_{k-1}h_k^tr_k ) ^ {-1 } kk=PK
x^k=x^k1kk(ykhkx^k1 ) ) )。
hat{x}_k=hat{x}_{k-1}+K_{k}(y_k-H_{k}hat{x}_{k-1}) x^k=x^k−1+Kk(yk−Hkx^k−1)协方差更新P k = ( I − K k H k ) P k − 1 ( I − K k H k ) T + K k R k K k T P_k=(I-K_kH_k)P_{k-1}(I-K_kH_k)^T+K_kR_kK_k^T Pk=(I−KkHk)Pk−1(I−KkHk)T+KkRkKkT 仿真背景
仿真场景为量一件物件的长和宽，真实物件的长宽分别为2米和3米，一共测量五十次，假定量测噪声为零均值的高斯噪声，且每次量测噪声之间是不相关的。根据更新的量测绘制出递推最现代的灯泡乘估计的过程动图。
Matlab程序 clc;clear;close allX_0 = [2;3]; N = 50; H = 1; R = [1 0;0 1];n = zeros(2,N); Y = zeros(2,N);X_e = zeros(2,N); P_e = zeros(2,2,N);K = zeros(2,2,N);error = zeros(2,N);h1 = subplot(221);h2 = subplot(222);h3 = subplot(223);h4 = subplot(224);for i = 1:N n(:,i) = mvnrnd([0;0],R,1); Y(:,i) = H*X_0 + n(:,i); plot(h1,Y(1,1:i),Y(2,1:i),'m.');title(h1,'位置观测和估计更新'); axis(h1,[0 4 1 5]); hold(h1,'on') if i == 1 X_e(:,1) = Y(:,1); P_e(:,:,1) = R; plot(h1,X_e(1,1),X_e(2,1),'bo'); error(:,1) = X_e(:,1) - X_0; else K(:,:,i) = P_e(:,:,i-1)*H'/(H*P_e(:,:,i-1)*H'+R); plot(h2,i,K(1,1,i),'b.');axis(h2,[0 N 0 0.6]);hold(h2,'on'); X_e(:,i) = X_e(:,i-1) + K(:,:,i)*(Y(:,i)-H*X_e(:,i-1)); P_e(:,:,i) = (eye(2)-K(:,:,i)*H)*P_e(:,:,i-1)*(eye(2)-K(:,:,i)*H)'+K(:,:,i)*R*K(:,:,i)'; plot(h1,X_e(1,i),X_e(2,i),'bo');title(h2,'横向位置校正增益'); error(:,i) = X_e(:,i) - X_0; plot(h3,1:i,abs(error(1,1:i)),'b-','LineWidth',1.2);axis(h3,[0 N 0 2]);title(h3,'横向位置估计误差');hold(h3,'on'); plot(h4,1:i,abs(error(2,1:i)),'b-','LineWidth',1.2);axis(h4,[0 N 0 2]);title(h4,'纵向位置估计误差');hold(h4,'on'); end hold(h1,'off') F = getframe(gcf); im = frame2im(F); [I,map] = rgb2ind(im,256); if i == 1 imwrite(I,map,'RLS.gif','GIF', 'Loopcount',inf,'DelayTime',0.3); else imwrite(I,map,'RLS.gif','GIF','WriteMode','append','DelayTime',0.3); end end 仿真动图
注意到增益的计算是可以离线计算的，给定了初始协方差矩阵，后续的增益也就确定了。