Utilizing a Kalman-Filter and IMM-L and IMM-CT for estimating the position of an object going through non-linear motion in matlab

Question

Currently I am working on the following problem that is based on the following section of my Estimation Theory textbook. This problem is attempting to compare a Kalman Filter to IMM-L and IMM-CT for a non-linear problem based on a object moving in a turn and requires several tasks to be performed.

Estimator designs (indicate units wherever applicable).

1. KF state eq, sampling interval, process noise standard deviation with rationale for the choice.

2. IMM-L Models 1, 2 state eqs, sampling interval, process noise standard deviations with units and rationale for their choices and Mode transition matrix.

3. IMM-CT Models 1, 2 state eqs, sampling interval, process noise standard deviations with units and rationale for their choices and Mode transition matrix.

4. Measurement model (coordinates and transformation) and measurement noise covariance used in filtering.

5. Initialization (state estimate and covariance matrix).

6. Plot from one run in the x-y plane: true position, measured position, KF, IMM-L and IMM-CT estimated position. Use appropriate marks to distinguish each and show the marks in the legend.

Plots from 100 Monte Carlo runs (as a function of time; indicate the ma- neuvering intervals by a thicker line on the time axis, indicate units wherever

applicable):

1. Position RMSE for: raw measurements, KF, IMM-L and IMM-CT. Use appropriate marks to distinguish each and show the marks in the legend.

2. Speed (magnitude of velocity vector) RMSE for KF, IMM-L and IMM-CT vs. the time index k.

3. Course (direction of velocity vector) RMSE for KF, IMM-L and IMM-CT. vs. the time index k.

4. Average maneuvering mode probabilities for IMM-L and IMM-CT vs. the time index k.

5. Normalized state estimation error squared for KF, IMM-L and IMM-CT (for the latter only the 4-dimensional position-velocity part of the state) with 2-sided 99% probability region vs. the time index k. Use appropriate marks to distinguish each and show the marks in the legend.

This is based on the text in the picture include with this question. [[[[[[[[enter image description here](https://i.sstatic.net/SnBnS.jpg)](https://i.sstatic.net/Yn2Xu.jpg)](https://i.sstatic.net/rMUw3.jpg)](https://i.sstatic.net/iDAUt.jpg)](https://i.sstatic.net/xT4En.jpg)](https://i.sstatic.net/3ZlaW.jpg)]

I have tried several things to get the code to work. I made two models and attempted to run the code but I am not certain that my results make sense for the situation that I am modeling this is the code that I have so far.

% Define Motion Models for IMM-CT
% Model 1: Constant velocity
F1 = [1, 0, sampling_period, 0, 0;
      0, 1, 0, sampling_period, 0;
      0, 0, 1, 0, 0;
      0, 0, 0, 1, 0;
      0, 0, 0, 0, 1];
H1 = [1, 0, 0, 0, 0;
      0, 1, 0, 0, 0];
% Model 2: Coordinated turn
omega = deg2rad(1); % Turn rate (1°/s)
F2 = @(dt) [1, 0, sin(omega*dt)/omega, -(1-cos(omega*dt))/omega, 0;
            0, 1, (1-cos(omega*dt))/omega, sin(omega*dt)/omega, 0;
            0, 0, cos(omega*dt), -sin(omega*dt), 0;
            0, 0, sin(omega*dt), cos(omega*dt), 0;
            0, 0, 0, 0, 1];
H2 = [1, 0, 0, 0, 0;
      0, 1, 0, 0, 0];

% Define Process Noise Covariance Matrices
% Adjusting noise variances for the fifth state
Q1 = eye(num_states);
m1 = 1; % Adjust as needed
Q1(3, 3) = m1; % x acceleration variance
Q1(4, 4) = m1; % y acceleration variance
Q1(5, 5) = m1; % omega variance
Q2 = eye(num_states);
m2 = 1; % Adjust as needed
Q2(3, 3) = m2;
Q2(4, 4) = m2;

% Define Mode Transition Matrix for IMM-CT
% Assuming equal probabilities for mode transitions
p_ij = 0.5; % Probability of transition from model i to model j
P = [p_ij, 1 - p_ij;
     1 - p_ij, p_ij];

% Define Measurement Noise Covariance
% Additive white Gaussian noise with covariance R = diag[2500 m^2, (1°)^2]
R = diag([2500, deg2rad(1)^2]);

% Define Initial State Estimate and Covariance Matrix for IMM-CT
initial_state_estimate_ct = [0; 0; 0; 250; 0]; % Initial state in Cartesian coordinates (x, y, x_dot, y_dot, omega)
initial_covariance_matrix_ct = eye(num_states); % Initial covariance matrix

% Initialize Arrays to Store Results
true_state = zeros(num_states, num_time_steps);
measurements = zeros(2, num_time_steps);
estimated_state_imm_l = zeros(num_states, num_time_steps); % Estimated state for IMM-L
estimated_state_imm_ct = zeros(num_states, num_time_steps, num_models); % Estimated state for IMM-CT
estimated_state_kf = zeros(num_states, num_time_steps); % Estimated state for Kalman Filter
covariance_matrix_imm_l = zeros(num_states, num_states, num_time_steps); % Covariance matrix for IMM-L
covariance_matrix_imm_ct = zeros(num_states, num_states, num_time_steps, num_models); % Covariance matrix for IMM-CT
covariance_matrix_kf = zeros(num_states, num_states, num_time_steps); % Covariance matrix for Kalman Filter
mode_probabilities_imm_l = zeros(num_models, num_time_steps + 1); % Mode probabilities for IMM-L
mode_probabilities_imm_ct = zeros(num_models, num_time_steps + 1); % Mode probabilities for IMM-CT
RMSPOS_mc_imm_l = zeros(num_runs, num_time_steps); % Position RMSE for IMM-L (Monte Carlo)
RMSPOS_mc_imm_ct = zeros(num_runs, num_time_steps); % Position RMSE for IMM-CT (Monte Carlo)
RMSPOS_mc_kf = zeros(num_runs, num_time_steps); % Position RMSE for Kalman Filter (Monte Carlo)
RMSVEL_mc_imm_l = zeros(num_runs, num_time_steps); % Speed RMSE for IMM-L (Monte Carlo)
RMSVEL_mc_imm_ct = zeros(num_runs, num_time_steps); % Speed RMSE for IMM-CT (Monte Carlo)
RMSVEL_mc_kf = zeros(num_runs, num_time_steps); % Speed RMSE for Kalman Filter (Monte Carlo)
RMSCRS_mc_imm_l = zeros(num_runs, num_time_steps); % Course RMSE for IMM-L (Monte Carlo)
RMSCRS_mc_imm_ct = zeros(num_runs, num_time_steps); % Course RMSE for IMM-CT (Monte Carlo)
RMSCRS_mc_kf = zeros(num_runs, num_time_steps); % Course RMSE for Kalman Filter (Monte Carlo)
NEES_mc_kf = zeros(num_runs, num_time_steps); % Normalized Estimation Error Squared for Kalman Filter (Monte Carlo)
NEES_mc_imm_l = zeros(num_runs, num_time_steps); % Normalized Estimation Error Squared for IMM-L (Monte Carlo)
NEES_mc_imm_ct = zeros(num_runs, num_time_steps); % Normalized Estimation Error Squared for IMM-CT (Monte Carlo)

% Simulate Target Movement and Perform Monte Carlo Simulations
for run = 1:num_runs
    % Reset true state and measurements for each run
    true_state(:, 1) = initial_state_estimate_ct;
    measurements(:, 1) = [norm(initial_state_estimate_ct(1:2) - [-104; 0]); atan2(initial_state_estimate_ct(2), initial_state_estimate_ct(1))];
    
    % Simulate target movement
    for k = 2:num_time_steps
        if k >= 11 && k <= 20 % Left turn with 1°/s for 10 seconds
            true_state(:, k) = F2(sampling_period) * true_state(:, k-1);
        elseif k > 20 && k <= 35 % Straight motion
            true_state(:, k) = F1 * true_state(:, k-1);
        elseif k > 35 && k <= 55 % Right turn with 1°/s for 15 seconds
            true_state(:, k) = F2(sampling_period) * true_state(:, k-1);
        else % Straight motion
            true_state(:, k) = F1 * true_state(:, k-1);
        end
        % Generate noisy measurements
        measurements(:, k) = [norm(true_state(1:2, k) - [-104; 0]); atan2(true_state(2, k), true_state(1, k))] + mvnrnd([0; 0], R)';
    end
    
    % Kalman Filter
    for k = 1:num_time_steps
        if k == 1
            estimated_state_kf(:, k) = initial_state_estimate_ct;
            covariance_matrix_kf(:, :, k) = initial_covariance_matrix_ct;
        else
            % Prediction step
            estimated_state_kf(:, k) = F1 * estimated_state_kf(:, k-1);
            covariance_matrix_kf(:, :, k) = F1 * covariance_matrix_kf(:, :, k-1) * F1' + Q1;
            
            % Update step
            K = covariance_matrix_kf(:, :, k) * H1' / (H1 * covariance_matrix_kf(:, :, k) * H1' + R);
            innovation = measurements(:, k) - H1 * estimated_state_kf(:, k);
            estimated_state_kf(:, k) = estimated_state_kf(:, k) + K * innovation;
            covariance_matrix_kf(:, :, k) = (eye(num_states) - K * H1) * covariance_matrix_kf(:, :, k);
        end
    end
    
    % IMM-CT
    for k = 2:num_time_steps+1
        % Compute mode probabilities
        mode_probabilities_imm_ct(:, k) = P * mode_probabilities_imm_ct(:, k-1);
        
        % Calculate IMM-CT estimate
        for i = 1:num_models
            % Prediction step
            if k == 2
                estimated_state_imm_ct(:, k-1, i) = initial_state_estimate_ct;
                covariance_matrix_imm_ct(:, :, k-1, i) = initial_covariance_matrix_ct;
            else
                if i == 1
                    estimated_state_imm_ct(:, k-1, i) = F1 * estimated_state_imm_ct(:, k-2, i);
                    covariance_matrix_imm_ct(:, :, k-1, i) = F1 * covariance_matrix_imm_ct(:, :, k-2, i) * F1' + Q1;
                else
                    estimated_state_imm_ct(:, k-1, i) = F2(sampling_period) * estimated_state_imm_ct(:, k-2, i);
                    covariance_matrix_imm_ct(:, :, k-1, i) = F2(sampling_period) * covariance_matrix_imm_ct(:, :, k-2, i) * F2(sampling_period)' + Q2;
                end
            end
            % Update step
            K = covariance_matrix_imm_ct(:, :, k-1, i) * H1' / (H1 * covariance_matrix_imm_ct(:, :, k-1, i) * H1' + R);
            innovation = measurements(:, k-1) - H1 * estimated_state_imm_ct(:, k-1, i);
            estimated_state_imm_ct(:, k-1, i) = estimated_state_imm_ct(:, k-1, i) + K * innovation;
            covariance_matrix_imm_ct(:, :, k-1, i) = (eye(num_states) - K * H1) * covariance_matrix_imm_ct(:, :, k-1, i);
        end
    end
    
    % IMM-L
    for k = 2:num_time_steps+1
        % Compute mode probabilities
        mode_probabilities_imm_l(:, k) = P * mode_probabilities_imm_l(:, k-1);
        
        % Calculate IMM-L estimate
        [~, index] = max(mode_probabilities_imm_l(:, k));
        if index == 1
            H = H1;
            Q = Q1;
        else
            H = H2;
            Q = Q2;
        end
        
        % Prediction step
        if k == 2
            estimated_state_imm_l(:, k-1) = initial_state_estimate_ct;
            covariance_matrix_imm_l(:, :, k-1) = initial_covariance_matrix_ct;
        else
            estimated_state_imm_l(:, k-1) = F1 * estimated_state_imm_l(:, k-2);
            covariance_matrix_imm_l(:, :, k-1) = F1 * covariance_matrix_imm_l(:, :, k-2) * F1' + Q1;
        end

        % Update step
        K = covariance_matrix_imm_l(:, :, k-1) * H' / (H * covariance_matrix_imm_l(:, :, k-1) * H' + R);
        innovation = measurements(:, k-1) - H * estimated_state_imm_l(:, k-1);
        estimated_state_imm_l(:, k-1) = estimated_state_imm_l(:, k-1) + K * innovation;
        covariance_matrix_imm_l(:, :, k-1) = (eye(num_states) - K * H) * covariance_matrix_imm_l(:, :, k-1);
    end
    
    % Compute Position RMSE for each estimator (Monte Carlo)
    for k = 1:num_time_steps
        % Kalman Filter
        error_kf = true_state(1:2, k) - estimated_state_kf(1:2, k);
        RMSPOS_mc_kf(run, k) = norm(error_kf);
        
        % IMM-CT
        for i = 1:num_models
            error_ct = true_state(1:2, k) - estimated_state_imm_ct(1:2, k, i);
            RMSPOS_mc_imm_ct(run, k) = RMSPOS_mc_imm_ct(run, k) + norm(error_ct);
        end
        
        % IMM-L
        error_l = true_state(1:2, k) - estimated_state_imm_l(1:2, k);
        RMSPOS_mc_imm_l(run, k) = norm(error_l);
        
        % Compute speed for true state and estimates
        true_speed = sqrt(true_state(3, k)^2 + true_state(4, k)^2);
        kf_speed = sqrt(estimated_state_kf(3, k)^2 + estimated_state_kf(4, k)^2);
        for i = 1:num_models
            imm_ct_speed = sqrt(estimated_state_imm_ct(3, k, i)^2 + estimated_state_imm_ct(4, k, i)^2);
        end
        
        % Compute course for true state and estimates
        true_course = atan2(true_state(4, k), true_state(3, k));
        kf_course = atan2(estimated_state_kf(4, k), estimated_state_kf(3, k));
        for i = 1:num_models
            imm_ct_course = atan2(estimated_state_imm_ct(4, k, i), estimated_state_imm_ct(3, k, i));
        end
        
        % Compute RMSE for speed and course
        RMSVEL_mc_kf(run, k) = abs(true_speed - kf_speed);
        RMSCRS_mc_kf(run, k) = abs(true_course - kf_course);
        for i = 1:num_models
            RMSVEL_mc_imm_ct(run, k) = abs(true_speed - imm_ct_speed);
            RMSCRS_mc_imm_ct(run, k) = abs(true_course - imm_ct_course);
        end
    end
    
    % Compute Normalized Estimation Error Squared (NEES) for Kalman Filter
    for k = 1:num_time_steps
        error_kf = true_state(:, k) - estimated_state_kf(:, k);
        NEES_mc_kf(run, k) = (error_kf' / covariance_matrix_kf(:, :, k)) * error_kf / num_states;
    end
    
    % Compute Normalized Estimation Error Squared (NEES) for IMM-L
    for k = 1:num_time_steps
        error_l = true_state(:, k) - estimated_state_imm_l(:, k);
        NEES_mc_imm_l(run, k) = (error_l' / covariance_matrix_imm_l(:, :, k)) * error_l / num_states;
    end
    
    % Compute Normalized Estimation Error Squared (NEES) for IMM-CT
    for k = 1:num_time_steps
        for i = 1:num_models
            error_ct = true_state(:, k) - estimated_state_imm_ct(:, k, i);
            NEES_mc_imm_ct(run, k) = (error_ct' / covariance_matrix_imm_ct(:, :, k, i)) * error_ct / num_states;
        end
    end
end

% Average RMSE over Monte Carlo runs for all estimators
mean_RMSPOS_mc_kf = mean(RMSPOS_mc_kf);
mean_RMSPOS_mc_imm_ct = mean(RMSPOS_mc_imm_ct);
mean_RMSPOS_mc_imm_l = mean(RMSPOS_mc_imm_l);
mean_RMSVEL_mc_kf = mean(RMSVEL_mc_kf);
mean_RMSVEL_mc_imm_ct = mean(RMSVEL_mc_imm_ct);
mean_RMSCRS_mc_kf = mean(RMSCRS_mc_kf);
mean_RMSCRS_mc_imm_ct = mean(RMSCRS_mc_imm_ct);
mean_mode_probabilities_imm_l = mean(mode_probabilities_imm_l, 1);
mean_mode_probabilities_imm_ct = mean(mode_probabilities_imm_ct, 1);
mean_NEES_mc_kf = mean(NEES_mc_kf);
mean_NEES_mc_imm_l = mean(NEES_mc_imm_l);
mean_NEES_mc_imm_ct = mean(NEES_mc_imm_ct);

% Plot Results
% Position RMSE
figure;
plot(0:num_time_steps-1, mean_RMSPOS_mc_kf, 'b--', 'LineWidth', 2);
hold on;
plot(0:num_time_steps-1, mean_RMSPOS_mc_imm_ct, 'r', 'LineWidth', 2);
plot(0:num_time_steps-1, mean_RMSPOS_mc_imm_l, 'g', 'LineWidth', 2);
xlabel('Time Step');
ylabel('Position RMSE (m)');
title('Position RMSE vs Time Step');
legend('Kalman Filter', 'IMM-CT', 'IMM-L', 'Location', 'best');
grid on;

% Speed RMSE
figure;
plot(0:num_time_steps-1, mean_RMSVEL_mc_kf, 'b--', 'LineWidth', 2);
hold on;
plot(0:num_time_steps-1, mean_RMSVEL_mc_imm_ct, 'r', 'LineWidth', 2);
plot(0:num_time_steps-1, zeros(1, num_time_steps), 'g', 'LineWidth', 2);
xlabel('Time Step');
ylabel('Speed RMSE');
title('Speed RMSE vs Time Step');
legend('Kalman Filter', 'IMM-CT', 'IMM-L', 'Location', 'best');
grid on;

% Course RMSE
figure;
plot(0:num_time_steps-1, mean_RMSCRS_mc_kf, 'b--', 'LineWidth', 2);
hold on;
plot(0:num_time_steps-1, mean_RMSCRS_mc_imm_ct, 'r', 'LineWidth', 2);
plot(0:num_time_steps-1, zeros(1, num_time_steps), 'g', 'LineWidth', 2);
xlabel('Time Step');
ylabel('Course RMSE');
title('Course RMSE vs Time Step');
legend('Kalman Filter', 'IMM-CT', 'IMM-L', 'Location', 'best');
grid on;

% Mode Probabilities
figure;
plot(0:num_time_steps, mean_mode_probabilities_imm_ct, 'r', 'LineWidth', 2);
hold on;
plot(0:num_time_steps, mean_mode_probabilities_imm_l, 'g', 'LineWidth', 2);
xlabel('Time Step');
ylabel('Average Mode Probability');
title('Average Mode Probabilities vs Time Step');
legend('IMM-CT', 'IMM-L', 'Location', 'best');
grid on;

% NEES
figure;
plot(0:num_time_steps-1, mean_NEES_mc_kf, 'b--', 'LineWidth', 2);
hold on;
plot(0:num_time_steps-1, mean_NEES_mc_imm_ct, 'r', 'LineWidth', 2);
plot(0:num_time_steps-1, mean_NEES_mc_imm_l, 'g', 'LineWidth', 2);
xlabel('Time Step');
ylabel('Normalized Estimation Error Squared (NEES)');
title('NEES vs Time Step');
legend('Kalman Filter', 'IMM-CT', 'IMM-L', 'Location', 'best');
grid on;

I do not believe that the IMM-L results are what they should be since they are just a straight line. So I am a bit at a loss currently and could really use some help with this problem.