ekf.m

扩展kalman 滤波的Matlab学习程序,特点是简单,能够设置输入参数,直观理解算法原理.

clear all
x = 0; % 初始状态
tf = 100; % 模拟长度
P(1) = 10;
k=1:tf;
u(k)=8 * cos(1.2*(k-1)); 
R=input('请输入过程噪声方差R的值: ');
Q=input('请输入观测噪声方差Q的值: ');
x(1)=x;
xpart(1)= x(1);
PArr(1) = P(1);
for k = 2 : tf
    % 模拟系统
    x(k) = 0.5 * x(k-1) + 25 * x(k-1) / (1 + x(k-1)^2) + u(k-1) + sqrt(R) * randn;
    y(k) = x(k)^2 / 20 + sqrt(Q) * randn;
   %扩展卡尔曼滤波
    xpartminus(k) = 0.5 * xpart(k-1) + 25 * xpart (k-1)/ (1 + xpart(k-1)^2) + u(k-1) + sqrt(R) * randn;
    ypart(k) = xpartminus(k)^2 / 20;
    vhat(k) = y (k)- ypart(k);%观测和预测的差
    A=0.5+25/(1+xpart(k-1)^2)-50*xpart(k-1)^2/(1+xpart(k-1)^2)^2;%f函数对x的雅可比式
    W=1;%f函数对k的雅可比式
    H=0.1*xpartminus(k);%h函数对x的雅可比式
    V=1;%h函数对n的雅可比式 
    PArr(k)=A*P(k-1)*A'+W*R*W';
    K=PArr(k)*H'*inv(H*PArr(k)*H'+Q);
    xpart(k)=xpartminus(k)+K*vhat(k);
    P(k)=PArr(k)-K*H*PArr(k);
% 在图片中表示出数据
end
P(tf)
close all;
t = 1 : tf;
figure
subplot(2,1,1)
plot(t,x(t),'b.',t,y(t),'g+',t,xpart(t),'k-');hold on;
set(gca,'FontSize',10); set(gcf,'Color','White'); 
xlabel('time step'); ylabel('state');
legend('真实值', '观测值',' EKF估计值'); 
subplot(2,1,2)
plot(t,P(t),'r')
set(gca,'FontSize',10); set(gcf,'Color','White'); 
xlabel('time step'); ylabel('error');
legend('滤波误差方差');