EKF localization (prediction only)

function [mu2, cov2] = ekf_localization(mu, cov, v, w, dt)
    % prediction only

    % noise model
    a1 = 0.01;
    a2 = 0;
    a3 = 0;
    a4 = 0.01;

    theta = mu(3);
    G = eye(3, 3);
    M = zeros(2, 2);

    G(1, 3) = -(v/w)*cos(theta) + (v/w)*cos(theta + w*dt);
    G(2, 3) = -(v/w)*sin(theta) + (v/w)*sin(theta + w*dt);

    V(1, 1) = (-sin(theta) + sin(theta + w*dt))/w;
    V(1, 2) = v*(sin(theta) - sin(theta + w*dt))/(w*w) + v*cos(theta + w*dt)*dt/w;
    V(2, 1) = (cos(theta) - cos(theta + w*dt))/w;
    V(2, 2) = -v*(cos(theta) - cos(theta + w*dt))/(w*w) + v*sin(theta + w*dt)*dt/w;
    V(3, 1) = 0;
    V(3, 2) = dt;

    M(1, 1) = a1*v*v + a2*w*w;
    M(2, 2) = a3*v*v + a4*w*w;

    mu2 = mu + [-(v/w)*sin(theta) + (v/w)*sin(theta + w*dt); (v/w)*cos(theta) - (v/w)*cos(theta + w*dt); w*dt];

    cov2 = G*cov*G' + V*M*V';
end

dt = 0.01;

mu = [0; 0; 0]; % initial state
cov = zeros(3, 3); % initial covariance
v = 1; % velocity, arbitary units
w = 45*pi/180; % angular velocity, rads/s

cov_hist = [];

for i=1:6000
    [mu2, cov2] = ekf_localization(mu, cov, v, w, dt);
    mu = mu2;
    cov = cov2;

    [vec, lambda] = eig(cov(1:2, 1:2));

    r1 = sqrt(lambda(1,1));
    r2 = sqrt(lambda(2,2));

    cov_hist = [cov_hist; i*dt pi*r1*r2];
end

figure(1);
clf;
hold on;
plot(cov_hist(:,1), cov_hist(:,2), 'r;constant velocity ;');
grid on

% with velocity inverted
mu = [0; 0; 0]; % initial state
cov = zeros(3, 3); % initial covariance
v = 1; % velocity, arbitary units
w = 45*pi/180; % angular velocity, rads/s

cov_hist = [];

for i=1:6000
    if i == 1000
        v = -v;
        %w = -w;
    end

    [mu2, cov2] = ekf_localization(mu, cov, v, w, dt);
    mu = mu2;
    cov = cov2;

    [vec, lambda] = eig(cov(1:2, 1:2));

    r1 = sqrt(lambda(1,1));
    r2 = sqrt(lambda(2,2));

    cov_hist = [cov_hist; i*dt pi*r1*r2];
end

figure(1);
plot(cov_hist(:,1), cov_hist(:,2), ';velocity inverted at t=10 ;');
xlabel("time (seconds)");
ylabel("covariance area for xy (pi*major*minor axis)");
title("EKF prediction only");
grid on
legend ("location", "northwest");