EKF举例

扩展卡尔曼滤波器的实验说明：

摘要，仅对非线性函数的卡尔曼滤波进行处理，效果图如下：

在使用的过程中，只需要将状态方程进行线性化处理即可，但是当滤波误差和一步预测误差较大的时候无法使用，也

即是过程噪声太大的时候，误差变大。

$$
X(k) = 0.5X(k-1)+\frac{2.5X(k-1)}{1+X^2(K-1)}+8cos(1.2k)+W(k)\

Y(k) = \frac{X^2(k)}{20}+V(k)
$$

程序代码

clc;
clear;
T = 50;
Q= 0.1;
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)/(1+x(k-1)^2)+8*cos(1.2*k)+w(k-1);
    y(k) = x(k)^2/20+v(k);
end

% 进行滤波算法的设计
X_ekf = zeros(1,T);
X_ekf(1)=x(1);
Y_ekf = zeros(1,T);
Y_ekf(1) = y(1);
P = zeros(1,T);
P(1) = 10;
for k =2:T
    Xn =  0.5*X_ekf(k-1)+2.5*X_ekf(k-1)/(1+X_ekf(k-1)^2)+8*cos(1.2*k);
    Zn = Xn^2/20;
    Phi = 0.5 +2.5*(1-Xn^2)/(1+Xn^2)^2;
    H = Xn/10;
    P_t = Phi*P(k-1)*Phi'+ Q;
    Kk = P_t*H'*inv(H*P_t*H'+R);
    X_ekf(k) = Xn+Kk*(y(k)-Zn);
    P(k) = (eye(1)-Kk*H)*P_t;    
end
X_std = zeros(1,T);
for k =1:T
    X_std(k) = abs(X_ekf(k)-x(k));
end

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

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

figure
hold on;box on;
plot(P);

程序输出结果：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-ZUCA8f6G-1649235347130)(E:\Icodetest\forMatlab\Kalman\ekf_img\untitled2.png)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-yJkuKWmR-1649235347130)(E:\Icodetest\forMatlab\Kalman\ekf_img\untitled1.png)]

Px的协方差滤波

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-eMgTHHoV-1649235347131)(E:\Icodetest\forMatlab\Kalman\ekf_img\untitled3.png)]