[Matlab] Estimating Parameters of Kalman Filter in Different Ways

kalman_filter_and_estimation.m

%% Kalman Filter Parameter Estimation Comparison with Log-Likelihood

% Clear workspace and set random seed for reproducibility
clear; clc; close all;
rng(1024);

%% Data Generation (Used by All Sections)

% Parameters
dim_x = 1; % Number of unobservable true states
dim_z = 1; % Dimension of observations
n_steps = 100; % Number of time steps
F = [1.0]; % State transition matrix
H = [1.0]; % Observation matrix
Q_true = [1.5]; % True process noise covariance
R_true = [2.8]; % True measurement noise covariance

% Generate true states and noisy observations
X = zeros(dim_x, n_steps); % True states
Z = zeros(dim_z, n_steps); % Observations
for t = 2:n_steps
    % True state evolution
    X(:, t) = F * X(:, t-1) + mvnrnd(zeros(1, dim_x), Q_true)';
    % Observation
    Z(:, t) = H * X(:, t) + mvnrnd(zeros(1, dim_z), R_true)';
end

% Initial state and covariance
x0 = zeros(dim_x, 1); % Initial state estimate
P0 = eye(dim_x);      % Initial covariance estimate

%% Section 1: Handwritten Kalman Filter and Parameter Estimation

% Handwritten Kalman Filter functions

% Prediction step
function [x_pred, P_pred] = kalman_predict(x, P, F, Q)
    x_pred = F * x;
    P_pred = F * P * F' + Q;
end

% Update step
function [x_upd, P_upd, S] = kalman_update(x_pred, P_pred, z, H, R)
    S = H * P_pred * H' + R;
    K = P_pred * H' / S;
    x_upd = x_pred + K * (z - H * x_pred);
    P_upd = (eye(size(P_pred)) - K * H) * P_pred;
end

% Negative log-likelihood function with explicit log(2π)
function nll = kalman_neg_log_likelihood_handwritten(params, Z, F, H, x0, P0)
    Q = params(1);
    R = params(2);
    if Q <= 0 || R <= 0
        nll = Inf;
        return;
    end
    dim_z = size(Z, 1); % Dimension of observations
    n_steps = size(Z, 2);
    x = x0;
    P = P0;
    nll = 0;
    for t = 1:n_steps
        % Prediction
        [x_pred, P_pred] = kalman_predict(x, P, F, Q);
        % Innovation
        y = Z(:, t) - H * x_pred;
        S = H * P_pred * H' + R;
        % Update negative log-likelihood (including log(2π))
        nll = nll + 0.5 * (dim_z * log(2 * pi) + log(det(S)) + y' * (S \ y));
        % Update state
        [x, P, ~] = kalman_update(x_pred, P_pred, Z(:, t), H, R);
    end
end

% Parameter estimation using fminsearch
initial_params = [1; 1]; % Initial guesses for [Q; R]
options = optimset('Display', 'off', 'TolX', 1e-6);
[estimated_params_handwritten, nll_handwritten] = fminsearch(@(params) kalman_neg_log_likelihood_handwritten(params, Z, F, H, x0, P0), initial_params, options);

% Extract estimated Q and R
Q_est_handwritten = estimated_params_handwritten(1);
R_est_handwritten = estimated_params_handwritten(2);

% Run Kalman filter with estimated parameters
x_est_handwritten = zeros(dim_x, n_steps);
P_est_handwritten = zeros(dim_x, dim_x, n_steps);
x = x0;
P = P0;
for t = 1:n_steps
    % Prediction
    [x_pred, P_pred] = kalman_predict(x, P, F, Q_est_handwritten);
    % Update
    [x, P, ~] = kalman_update(x_pred, P_pred, Z(:, t), H, R_est_handwritten);
    % Store estimates
    x_est_handwritten(:, t) = x;
    P_est_handwritten(:, :, t) = P;
end

%% Section 2: Kalman Filter Using ss Model and Handwritten Log-Likelihood

% Define the state-space system using ss
sys = ss(F, [], H, [], 1); % Discrete-time system with sample time 1

% Negative log-likelihood function with explicit log(2π)
function nll = kalman_neg_log_likelihood_ss(params, sys, x0, P0, Z)
    Q = params(1);
    R = params(2);
    if Q <= 0 || R <= 0
        nll = Inf;
        return;
    end
    dim_z = size(Z, 1); % Dimension of observations
    x = x0;
    P = P0;
    nll = 0;
    for t = 1:size(Z, 2)
        % Prediction
        x_pred = sys.A * x;
        P_pred = sys.A * P * sys.A' + Q;
        % Innovation
        y = Z(:, t) - sys.C * x_pred;
        S = sys.C * P_pred * sys.C' + R;
        % Update negative log-likelihood (including log(2π))
        nll = nll + 0.5 * (dim_z * log(2 * pi) + log(det(S)) + y' * (S \ y));
        % Update state
        K = P_pred * sys.C' / S;
        x = x_pred + K * y;
        P = (eye(size(P)) - K * sys.C) * P_pred;
    end
end

% Parameter estimation using fminsearch
initial_params = [1; 1]; % Initial guesses for [Q; R]
[estimated_params_ss, nll_ss] = fminsearch(@(params) kalman_neg_log_likelihood_ss(params, sys, x0, P0, Z), initial_params, options);

% Extract estimated Q and R
Q_est_ss = estimated_params_ss(1);
R_est_ss = estimated_params_ss(2);

% Run Kalman filter with estimated parameters
x_est_ss = zeros(dim_x, n_steps);
P_est_ss = zeros(dim_x, dim_x, n_steps);
x = x0;
P = P0;
for t = 1:n_steps
    % Prediction
    x_pred = sys.A * x;
    P_pred = sys.A * P * sys.A' + Q_est_ss;
    % Innovation
    y = Z(:, t) - sys.C * x_pred;
    S = sys.C * P_pred * sys.C' + R_est_ss;
    % Update state
    K = P_pred * sys.C' / S;
    x = x_pred + K * y;
    P = (eye(size(P)) - K * sys.C) * P_pred;
    % Store estimate
    x_est_ss(:, t) = x;
    P_est_ss(:, :, t) = P;
end

%% Section 3: Kalman Filter Using ssm and Built-in Functions

% Define parameter-to-matrix mapping function
function [A, B, C, D, Mean0, Cov0, StateType] = LocalStateSpaceModel(params)
    Q = params(1);
    R = params(2);
    % State-space matrices
    A = 1;          % State transition matrix
    B = sqrt(Q);    % Process noise effect
    C = 1;          % Observation matrix
    D = sqrt(R);    % Measurement noise effect
    % Initial state
    Mean0 = 0;
    Cov0 = 1;
    % State type (0 for stationary)
    StateType = 0;
end

% Initial parameter guesses
Param0 = [1; 1]; % Initial guesses for [Q; R]

% Create ssm model using the mapping function
Mdl = ssm(@LocalStateSpaceModel);

% Estimate the parameters using built-in functions
Z_transpose = Z';
[EstMdl, EstParams, EstParamCov, logL] = estimate(Mdl, Z_transpose, Param0, 'Display', 'off');

% Extract estimated Q and R
Q_est_ssm = EstParams(1);
R_est_ssm = EstParams(2);

% Extract log-likelihood value
nll_ssm = -logL; % Negative log-likelihood

% Run Kalman filter with estimated parameters
x_est_ssm = zeros(dim_x, n_steps);
P_est_ssm = zeros(dim_x, dim_x, n_steps);
x = EstMdl.Mean0;
P = EstMdl.Cov0;

% Extract estimated state-space matrices
A = EstMdl.A;
C = EstMdl.C;
Q_est_matrix = EstMdl.B * EstMdl.B'; % Process noise covariance
R_est_matrix = EstMdl.D * EstMdl.D'; % Measurement noise covariance

for t = 1:n_steps
    % Prediction
    x_pred = A * x;
    P_pred = A * P * A' + Q_est_matrix;
    % Innovation
    y = Z(:, t) - C * x_pred;
    S = C * P_pred * C' + R_est_matrix;
    % Update state
    K = P_pred * C' / S;
    x = x_pred + K * y;
    P = (eye(dim_x) - K * C) * P_pred;
    % Store estimates
    x_est_ssm(:, t) = x;
    P_est_ssm(:, :, t) = P;
end

%% Comparison of Results

% Plot the true state, observations, and estimated states from all methods
figure;
plot(1:n_steps, X(1, :), 'k-', 'LineWidth', 2, 'DisplayName', 'True State');
hold on;
plot(1:n_steps, Z(1, :), 'ko', 'MarkerSize', 4, 'DisplayName', 'Observations');

plot(1:n_steps, x_est_handwritten(1, :), 'r--', 'LineWidth', 1.5, 'DisplayName', 'Estimated State (Handwritten)');
plot(1:n_steps, x_est_ss(1, :), 'b-.', 'LineWidth', 1.5, 'DisplayName', 'Estimated State (ss Model)');
plot(1:n_steps, x_est_ssm(1, :), 'g:', 'LineWidth', 1.5, 'DisplayName', 'Estimated State (ssm Model)');

xlabel('Time Step');
ylabel('State Value');
title('Kalman Filter: Parameter Estimation with Different Methods');
legend('Location', 'best');
grid on;
hold off;

% Display estimated parameters and negative log-likelihoods from all methods
fprintf('--- Parameter Estimation Results ---\n');
fprintf('True Process Noise Covariance Q_true: %.4f\n', Q_true);
fprintf('Estimated Q (Handwritten): %.4f\n', Q_est_handwritten);
fprintf('Estimated Q (ss Model): %.4f\n', Q_est_ss);
fprintf('Estimated Q (ssm Model): %.4f\n\n', Q_est_ssm);

fprintf('True Measurement Noise Covariance R_true: %.4f\n', R_true);
fprintf('Estimated R (Handwritten): %.4f\n', R_est_handwritten);
fprintf('Estimated R (ss Model): %.4f\n', R_est_ss);
fprintf('Estimated R (ssm Model): %.4f\n\n', R_est_ssm);

fprintf('--- Negative Log-Likelihoods ---\n');
fprintf('Negative Log-Likelihood (Handwritten): %.4f\n', nll_handwritten);
fprintf('Negative Log-Likelihood (ss Model): %.4f\n', nll_ss);
fprintf('Negative Log-Likelihood (ssm Model): %.4f\n', nll_ssm);

Similar results. So I guess I can just use my handwritten ones, easier to understand.

--- Parameter Estimation Results ---
True Process Noise Covariance Q_true: 1.5000
Estimated Q (Handwritten): 1.6873
Estimated Q (ss Model): 1.6873
Estimated Q (ssm Model): 1.6873


True Measurement Noise Covariance R_true: 2.8000
Estimated R (Handwritten): 2.9424
Estimated R (ss Model): 2.9424
Estimated R (ssm Model): 2.9424


--- Negative Log-Likelihoods ---
Negative Log-Likelihood (Handwritten): 232.7316
Negative Log-Likelihood (ss Model): 232.7316
Negative Log-Likelihood (ssm Model): 232.7316