简单非线性扩展Kalman滤波器设计

matlab程序

function EFK1
T=50;
Q=10;
R=1;
w=sqrt(Q)*randn(1,T);
v=sqrt(R)*randn(1,T);
x=zeros(1,T);
x(1)=0.1;
y=zeros(1,T);
y(1)=x(1)^2/20+v(1);
for k=2:T
    x(k)=0.5*x(k-1)+2.5*x(k-1)^2/(1+x(k-1)^2)+8*cos(1.2*k)+w(k);
    y(k)=x(k)^2/20+v(k);
end
Xekf=zeros(1,T);
Xekf(1)=x(1);
Yekf=zeros(1,T);
Yekf(1)=y(1);
P0=eye(1);
for k=2:T
    Xn=0.5*Xekf(k-1)+2.5*Xekf(k-1)/(1+Xekf(k-1)^2)+8*cos(1.2*k);
    Zn=Xn^2/20;
    F=0.5+2.5*(1-Xn^2)/(1+Xn^2)^2;
    H=Xn/10;
    P=F*P0*F'+Q;
    K=P*H'*inv(H*P*H'+R);
    Xekf(k)=Xn+K*(y(k)-Zn);
    P0=(eye(1)-K*H)*P;
end

Xstd=zeros(1,T);
for k=1:T
    Xstd(k)=abs(Xekf(k)-x(k));
end

figure
hold on;box on;
plot(x,'-ko','MarkerFaceColor','g')
plot(Xekf,'-ks','MarkerFaceColor','b')
legend('真实值','Kalman滤波估计值')
xlabel('时间/s')
ylabel('状态值x')

figure
hold on;box on;
plot(Xstd,'-ko','MarkerFaceColor','g');
xlabel('时间/s')
ylabel('状态估计偏差')

 

Q=10 

 Q=0.1