by An Uncommon Lab

hw1ans1

function hw1ans1()
 
% Homework 1, Answer 1: The KFF Method
%
% This function solves homework problem 1 using the Kalman Filter
% Framework tools. All of the work is contained in this single file.
%
% It first defines the elements of the dynamics and observation functions
% and then sets up some options, runs the filter, and runs a Monte-Carlo
% test.
%
% See skfexample('hw1') for more.
 
% Copyright 2016 An Uncommon Lab
 
    % Constants for the dynamics
    dt = 0.25; % Time step [s]
    m  = 10;   % Mass [kg]
    k  = 5;    % Spring constant [N/m]
    b  = 2;    % Spring damping  [N/(m/s)]
    
    % Continuous-time Jacobian for the linear system: x_dot = A * x
    A = [0 1; -k/m, -b/m];
    
    % Discrete-time Jacobian: x_k = F * x_km1
    constants.F = expm(A * dt);
 
    % The process noise consists of a single acceleration parameter, q. 
    % This acceleration maps to the change in position and velocity as:
    % 
    %   dp = 0.5 * dt^2 * q
    %   dv = dt * q
    % 
    % Or, using the state vector x = [p; v]:
    %
    %   dx = [0.5 * dt^2; dt] * q = Fq * q
    %
    % The covariance of dx is (using E(.) as the expectation operator):
    %
    %   E(dx * dx.') = E(Fq * q * q.' * Fq.') 
    %                = Fq * E(q*q.') * Fq.' 
    %                = Fq * Q * Fq.'
    % 
    % where Q is the given process noise variance (0.1 m/s^2)^2.
    % 
    Fq = [0.5*dt^2, dt];
    Q  = Fq * 0.1^2 * Fq.';
    
    % We observe the position only.
    H = [1 0];
    
    % The measurement noise variance is given as m^2.
    R = 0.1^2;
 
    % The initial estimate and covariance are given.
    x_hat_0 = [1; 0];      % [m,   m/s]
    P_0     = diag([1 2]); % [m^2, m^2/s^2]
    
    % Create the kff options structure.
    options = kffoptions('f',         @propagate, ...
                         'F_km1_fcn', constants.F, ...
                         'Q_km1_fcn', Q, ...
                         'h',         @observe, ...
                         'H_k_fcn',   H, ...
                         'R_k_fcn',   R);
                     
	% Let's try a single simulation first.
	[t, x, x_hat] = kffsim([0 30], dt, x_hat_0, P_0, options, ...
                           'UserVars', {constants});
    
    % Plot the results to make sure things look good.
    unique_figure(mfilename());
    subplot(2, 1, 1);
    plot(t, x(1, :), t, x_hat(1, :));
    ylabel('Position');
    subplot(2, 1, 2);
    plot(t, x(2, :), t, x_hat(2, :));
    ylabel('Velocity');
                          
	% That looks good, so let's try a Monte-Carlo test.
	kffmct(100, [0 30], dt, x_hat_0, P_0, options, ...
           'UserVars', {constants});
 
end % hw1ans1
 
% The propagation function is actually linear, so this part is very easy.
function x_k = propagate(t_km1, t_k, x_km1, u_km1, constants) %#ok
 
    x_k = constants.F * x_km1;
    
end % propagate
 
% The observation function just returns the position.
function z_k = observe(t_k, x_k, u_k, constants) %#ok
 
    z_k = x_k(1);
    
end % observe

NEES: Percent of data in theoretical 95.0% bounds: 95.0%
NMEE: Percent of data in theoretical 95.0% bounds: 94.6%
NIS:  Percent of data in theoretical 95.0% bounds: 95.0%
TAC:  Percent of data in theoretical 95.0% bounds: 96.6%
AC:   Percent of data in theoretical 95.0% bounds: 95.1%