| % author : Perry.Li @USTC% function: simulating the process of EKF% date: 04/28/2015% N = 50; %計算連續(xù)N個時刻 n=3; %狀態(tài)維度q=0.1; %過程標(biāo)準(zhǔn)差r=0.2; %測量標(biāo)準(zhǔn)差Q=q^2*eye(n); %過程方差R=r^2; %測量值的方差 f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; %狀態(tài)方程h=@(x)[x(1);x(2);x(3)]; %測量方程s=[0;0;1]; %初始狀態(tài)%初始化狀態(tài)x=s+q*randn(3,1); P = eye(n); xV = zeros(n,N); sV = zeros(n,N); zV = zeros(n,N);for k=1:N z = h(s) + r*randn; sV(:,k)= s; %實際狀態(tài) zV(:,k) = z; %狀態(tài)測量值 [x1,A]=jaccsd(f,x); %計算f的雅可比矩陣,，其中x1對應(yīng)黃金公式line2 P=A*P*A'+Q; %過程方差預(yù)測,，對應(yīng)line3 [z1,H]=jaccsd(h,x1); %計算h的雅可比矩陣 K=P*H'*inv(H*P*H'+R); %卡爾曼增益，對應(yīng)line4 x=x1+K*(z-z1); %狀態(tài)EKF估計值,，對應(yīng)line5 P=P-K*H*P; %EKF方差,，對應(yīng)line6 xV(:,k) = x; %save s = f(s) + q*randn(3,1); %update process endfor k=1:3 FontSize=14; LineWidth=1; figure(); plot(sV(k,:),'g-'); %畫出真實值 hold on; plot(xV(k,:),'b-','LineWidth',LineWidth) %畫出最優(yōu)估計值 hold on; plot(zV(k,:),'k+'); %畫出狀態(tài)測量值 hold on; legend('真實狀態(tài)', 'EKF最優(yōu)估計估計值','狀態(tài)測量值'); xl=xlabel('時間(分鐘)'); t=['狀態(tài) ',num2str(k)] ; yl=ylabel(t); set(xl,'fontsize',FontSize); set(yl,'fontsize',FontSize); hold off; set(gca,'FontSize',FontSize);end |
|
|
