扩展卡尔曼滤波

最新推荐文章于 2024-09-12 19:02:20 发布

烟雨楼听烟雨

最新推荐文章于 2024-09-12 19:02:20 发布

阅读量305

点赞数 5

文章标签：算法

本文链接：https://blog.csdn.net/J976918/article/details/136789434

版权

扩展卡尔曼滤波（Extended Kalman Filter，EKF）是卡尔曼滤波的一种扩展，用于非线性系统的状态估计。与标准卡尔曼滤波不同，EKF通过在线性化非线性系统模型和观测模型来处理非线性性质。

下面给出二维平面匀速直线运动扩展卡尔曼滤波仿真。其中观测为三个距离的模型。

clc;clear;close all;
T=0.1;
N=100;
X=zeros(4,N);
X(:,1)=[0,2,0,2];%x,vx,y,vy
delta_w1=1e-2;
Q=delta_w1*diag([1,1]);
G=[0.5*T^2  ,   0   ;
      T     ,   0   ;
      0     ,0.5*T^2;
      0     ,   T   ];
 delta_w2=1e-2;
 R=delta_w2*diag([1,2,1]);
 F=[1 , T , 0 , 0;
    0 , 1 , 0 , 0;
    0 , 0 , 1 , T;
    0 , 0 , 0 , 1];
%% 三个基站坐标位置
x1=4;y1=0;
x2=10;y2=0;
x3=7;y3=3*3^0.5;
%% 模拟轨迹
for t=2:N
    X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);
end
%% 观测方程
for t=1:N
    Z(1,t)=sqrtm((X(1,t)-x1)^2+(X(3,t)-y1)^2);
    Z(2,t)=sqrtm((X(1,t)-x2)^2+(X(3,t)-y2)^2);
    Z(3,t)=sqrtm((X(1,t)-x3)^2+(X(3,t)-y3)^2);
    Z(:,t)=Z(:,t)+sqrtm(R)*randn(3,1);           
end
%% 集中式EKF
Xekf=zeros(4,N);
Xekf(:,1)=X(:,1);
P0=eye(4);
for i=2:N
    Xn(:,i-1)=F*Xekf(:,i-1);
    P1=F*P0*F'+G*Q*G';
    dd(:,i-1)=[sqrtm((Xn(1,i-1)-x1)^2+(Xn(3,i-1)-y1)^2);sqrtm((Xn(1,i-1)-x2)^2+(Xn(3,i-1)-y2)^2);sqrtm((Xn(1,i-1)-x3)^2+(Xn(3,i-1)-y3)^2)];
    H=[(Xn(1,i-1)-x1)/dd(1,1),0,(Xn(3,i-1)-y1)/dd(1,1),0;
       (Xn(1,i-1)-x2)/dd(2,1),0,(Xn(3,i-1)-y2)/dd(2,1),0;
       (Xn(1,i-1)-x3)/dd(3,1),0,(Xn(3,i-1)-y3)/dd(3,1),0];
   K=P1*H'*inv(H*P1*H'+R);
   Xekf(:,i)=Xn(:,i-1)+K*(Z(:,i)-dd(:,i-1));
   P0=(eye(4)-K*H)*P1;
end

figure
hold on;box on;
plot(X(1,:),X(3,:),'-k.');
plot(Xekf(1,:),Xekf(3,:),'-r+');
legend('真实轨迹','EKF轨迹')