Frameworks

*kf includes a Kalman filter framework that allows one to take advantage of many filtering ideas quickly and immediately simulate the results. For instance, here in a matter of lines is the definition of a linear Kalman filter and the code to run a Monte-Carlo test of its performance:

% Define the system, Jacobians, noise covariance, and initial conditions.
dt = 0.05;                         % Time step
F  = expm([0 1; 2 0] * dt);        % Discrete state transition matrix
G  = [0.5*dt^2; dt];               % Input-to-state map
f  = @(t0, tf, x, u) F*x + G*u;    % Linear dynamics
u  = @(t, x, z) -1/dt*[1 0.1] * x; % Function to produce control vector, u
h  = @(t, x, u) x(1);              % Observation function
H  = [1 0];                        % Sensitivity matrix
Q  = G * 0.1^2 * G.';              % Process noise covariance
R  = 0.1^2;                        % Measurement noise covariance
x_hat_0 = [1; 0];                  % Initial state
P_0     = diag([1 0.5].^2);        % Initial covariance
 
% Create the options for the filter.
options = kffoptions('f',         f, ...
                     'F_km1_fcn', F, ...
                     'Q_km1_fcn', Q, ...
                     'h',         h, ...
                     'H_k_fcn',   H, ...
                     'R_k_fcn',   R);
 
% Simulate once.
kffsim([0 5], dt, x_hat_0, P_0, u, options);
 
% Simulate 100 times as part of a Monte-Carlo test. Each run will use a
% different initial true state based around x_hat_0, consistent with P_0.
kffmct(100, [0 10], dt, x_hat_0, x_hat_0, P_0, u, options);

The simulations will use the propagation and observation functions along with the process noise covariance and measurement noise covariance to propatate a true state and create noisy measurements. The noisy measurements will be provided to the filter function, kff, over time. When the runs are completed, we'll see plots of the true state, estimates, measurements, and several statistical outputs to determine if the filter is behaving well.

Of course, the linear problem above is just a toy, and the framework functions are far more useful on more complex problems. For instance, to create an extended Kalman filter, we simply set F_km1_fcn in kffoptions to a function handle instead of a constant value like we used above. For instance, here's a mix of function handles, anonymous functions, and constants to define a problem.

options = kffoptions('f',         @my_propagation_function, ...
                     'F_km1_fcn', @my_jacobian_function, ...
                     'Q_km1_fcn', @my_process_noise_function, ...
                     'h',         @(~, x, ~) x(1:2), ...
                     'H_k_fcn',   [eye(2), zeros(2)], ...
                     'R_k_fcn',   0.1^2 * eye(2));
 
% Simulate it and get the true state and estimated state over time.
[t, x, x_hat, P] = kffsim([0 5], dt, x_hat_0, P_0, options);

For a complete example of an extended Kalman filter, here we create one for a thrown ball which experiences drag. The propagation function is now nonlinear, and its Jacobian can be determined at every step of the filter.

% Propagation function
dt = 0.05;                            % Time step
g  = [0; -9.81];                      % Gravity
T  = [dt*eye(2), 0.5*dt^2*eye(2); ... % Integrates [vel;acc] to [pos;vel].
       zeros(2),       dt*eye(2)];
f  = @(t0, tf, x, u) x + T * [x(3:4); g - 0.01*norm(x(3:4))*x(3:4)];
       
% Function to return Jacobian df/dX
dadv = @(x) -0.01 * (x(3:4)*x(3:4).'*1/norm(x(3:4)) + norm(x(3:4))*eye(2));
F    = @(t, tf, x, u) eye(4) + T * [zeros(2), eye(2); zeros(2), dadv(x)];
 
h       = @(t, x, u) x(1:2);           % Observation function
H       = [eye(2), zeros(2)];          % Sensitivity matrix
Q       = T(:,3:4) * 1^2 * T(:,3:4).'; % Process noise covariance
R       = diag([5 5].^2);              % Measurement noise covariance
x_hat_0 = [0; 0; 25; 25];              % Initial state
P_0     = diag([5 5 10 10].^2);        % Initial covariance
 
% Create the options for the filter.
options = kffoptions('f',         f, ...
                     'F_km1_fcn', F, ...
                     'Q_km1_fcn', Q, ...
                     'h',         h, ...
                     'H_k_fcn',   H, ...
                     'R_k_fcn',   R);
 
% Simulate once.
kffsim([0 4], dt, x_hat_0, P_0, options);
 
% Simulate 100 times as part of a Monte-Carlo test.
kffmct(100, [0 4], dt, x_hat_0, P_0, options);

This framework can be used to quickly construct filters with iteration, consider covariance, and sequential (scalar) updates, and the Monte-Carlo and simulation functions allow different propgation and measurement functions to be used than are used by the filter.

Next Steps

To learn more about using the framework functions, see the documentation for the individual functions below or have a look at the frameworks tutorial, Mr. Eustace's Jump.

The Kalman filter framework includes:

kff to perform the filtering for a single step
kffoptions to define the filter's options and constants
kffsim to run a single simulation of the filter
kffmct to run numerous simulations of the filter and view statistical results

To use generic filtering functions, simpler than the framework functions, see Basic Filters.

Docs

Frameworks

Next Steps