扩展卡尔曼EKF——估计匀速运动小车的位置和速度，高动态作业matlab

数据源为.mat文件，点击这里免费下载

% 用扩展卡尔曼EKF滤波算法估计匀速运动小车的位置和速度

load('data.mat');
Q = [3, 0; 0, 4];
P0 = eye(4);
R = 2;
x0 = 100;
y0 = 120;
x = data(:,5);          % x轴方向位移
y = data(:,6);          % y轴方向位移
vx = data(:,2);	        % x方向分速度
vy = data(:,3);	        % y方向分速度
dist = data(:,4);       % 距离dist
t = data(:,1);          % 时刻t
Xekf = [x(1); y(1); vx(1); vy(1)];      % 状态矩阵X初值 4x1

for k = 2:size(data)
    Fai = [1,0,t(k-1),0; 0,1,0,t(k-1); 0,0,1,0; 0,0,0,1];% 状态转移矩阵4x4
    G = [t(k-1),0; 0,t(k-1); 1,0; 0,1]; % 噪声系数矩阵
    Xesti = Fai * Xekf;                 % 先验估计
    P = Fai*P0*Fai' + G*Q*G';           % 计算先验估计的协方差
    H = [(Xesti(1)-x0) / sqrt((Xesti(1)-x0)^2+(Xesti(2)-y0)^2), (Xesti(2)-y0) / sqrt((Xesti(1)-x0)^2+(Xesti(2)-y0)^2), 0, 0];   % 按公式'-'不对？
    K = P * H' * inv(H *P *H' + R);     % 计算卡尔曼增益
    Zk = sqrt((Xesti(1)-x0)^2 + (Xesti(2)-y0)^2);        % 观测距离，按公式'-'不对？
    Xekf = Xesti + K * (dist(k) - Zk)   % 最优估计 = 先验估计 + K观测值
    P0 = (eye(4) - K*H) * P;            % 更新最优估计的协方差矩阵

    % plot变量
    xe(k) = Xesti(1);
    ye(k) = Xesti(2);
    vxe(k) = Xesti(3);
    vye(k) = Xesti(4);
    xk(k) = Xekf(1);
    yk(k) = Xekf(2);
    vxk(k) = Xekf(3);
    vyk(k) = Xekf(4);
    diste(k) = Zk;
end

% 画出估计轨迹和真实轨迹的对比图；画出估计速度和测量速度的对比图。
figure(1);
plot(xe, ye, 'b-.');hold on;
plot(x, y, 'r--');  hold on;
plot(xk, yk, 'g-'); hold off;
legend('先验估计轨迹', '观测轨迹', '真实轨迹-最优估计');
figure(2);
plot(vxe, vye, 'b-.');hold on;
plot(vx, vy, 'r--');  hold on;
plot(vxk, vyk, 'g-'); hold off;
legend('先验估计速度', '观测速度', '真实速度-最优估计');
figure(3);
plot(t, xe, 'b-.');hold on;
plot(t, x, 'r--'); hold on;
plot(t, xk, 'g-'); hold off;
legend('x位移-先验估计', 'x位移-观测值', 'x位移-最优估计');
figure(4);
plot(t, ye, 'b-.');hold on;
plot(t, y, 'r--'); hold on;
plot(t, yk, 'g-'); hold off;
legend('y位移-先验估计', 'y位移-观测值', 'y位移-最优估计');
figure(5);
plot(t, vxe, 'b-.');hold on;
plot(t, vx, 'r--'); hold on;
plot(t, vxk, 'g-'); hold off;
legend('x速度-先验估计', 'x速度-观测值', 'x速度-最优估计');
figure(6);
plot(t, vye, 'b-.');hold on;
plot(t, vy, 'r--'); hold on;
plot(t, vyk, 'g-'); hold off;
legend('y速度-先验估计', 'y速度-观测值', 'y速度-最优估计');

Fig1：轨迹的 先验估计、观测值、最优估计

Fig2：合速度的 先验估计、观测值、最优估计

-------------------------------------------Fig3~6 不必要-------------------------------------------

Fig.3：x方向位移的 先验估计、观测值、最优估计

Fig.4：y方向位移的 先验估计、观测值、最优估计

Fig.5：x方向分速度的 先验估计、观测值、最优估计

Fig.6：y方向分速度的 先验估计、观测值、最优估计