Run one step of a linear Kalman filter for a discrete linear system described by:
x_k = F_km1 * x_km1 + F_u_km1 * u_km1 + w_q z_k = H_k * x_k + w_r
w_q is a zero-mean Gaussian random variable
with zero mean and covariance
Q_km1. That is,
w_q ~ N(0, Q_km1).
w_r ~ N(0, R_k).
It updates a state estimate and covariance from sample k-1 to sample k.
[x_k, P_k] = lkf(x_km1, P_km1, u_km1, z_k, ... F_km1, F_u_km1, H_k, Q_km1, R_k);
State estimate at sample k-1
State estimate covariance at sample k-1
Input vector at sample k-1
Measurements at sample k
State transition matrix at sample k-1
Input-to-state-update matrix for sample k-1
Observation matrix at sample k
Process noise at sample k
Measurement noise at sample k
Updated state estimate for sample k
Updated state covariance for sample k
We can quickly create a simulation for discrete, dynamic system, generate noisy measurements of the system over time, and pass these to a linear Kalman filter.
First, define the discrete system.
rng(1); dt = 0.1; % Time step F_km1 = expm([0 1; -1 0]*dt); % State transition matrix H_k = [1 0]; % Observation matrix Q_km1 = 0.5^2 * [0.5*dt^2; dt]*[0.5*dt^2 dt]; % Process noise covariance R_k = 0.1; % Meas. noise covariance
Now, we'll define the simulation's time step and initial conditions. Note that we define the initial estimate and set the truth as a small error from the estimate (using the covariance).
n = 100; % Number of samples to simulate x_hat_0 = [1; 0]; % Initial estimate P = diag([0.5 1].^2); % Initial estimate covariance x_0 = x_hat_0 + mnddraw(P, 1); % Initial true state
Now we'll just write a loop for the discrete simulation.
% Storage for time histories x = [x_0, zeros(2, n-1)]; % True state x_hat = [x_hat_0, zeros(2, n-1)]; % Estimate z = [H_k * x_0 + mnddraw(R_k, 1), zeros(1, n-1)]; % Measurement % Simulate each sample over time. for k = 2:n % Propagate the true state. x(:, k) = F_km1 * x(:, k-1) + mnddraw(Q_km1, 1); % Create the real measurement at sample k. z(:, k) = H_k * x(:, k) + mnddraw(R_k, 1); % Run the Kalman correction. [x_hat(:, k), P] = lkf(x_hat(:, k-1), P, , z(:, k), ... F_km1, , H_k, Q_km1, R_k); end
Plot the results.
figure(1); clf(); t = 0:dt:(n-1)*dt; h = plot(t, x, ... t, z, '.', ... t, x_hat, '--'); legend('True x1', 'True x2', 'Meas.', 'Est. x1', 'Est. x2'); xlabel('Time');