by An Uncommon Lab

hw2ans1

function hw2ans1()
 
% hw2ans1
%
% This function demonstrates how to solve homework problem 2 using the
% Kalman Filter Framework tools. All of the work is contained in this
% single file, showing how quickly one can use kff to create a filter
% prototype.
%
% See skfexample('hw2') for more.
 
% Copyright 2016 An Uncommon Lab
 
    % Set up the initial conditions directly from hw1.m.
    P_0     = (0.05)^2 * eye(3);
    x_hat_0 = [0; 0; 0];
    
    % Set up some constants related to the dynamics.
    dt = 1;   % We update discretely; let dt be 1 for "one step".
    u  = 1;   % Generalized application force
    a  = 0.1; % Constant mapping generalized force to generalized pressure
    
    % The only constant for this filter is the measurement noise
    % covariance.
    R = 0.003^2 * eye(3);
    
    % Set up the options for kff, telling it which functions to call for
    % propagation, observation, process noise, and the various Jacobians.
    options = kffoptions('f',         @propagate, ...
                         'F_km1_fcn', @propagation_jacobian, ...
                         'Q_km1_fcn', @process_noise, ...
                         'h',         @observe, ...
                         'H_k_fcn',   @observation_jacobian, ...
                         'R_k_fcn',   R);
    
    % Run a simulation for 300 applications. Pass our 'a' variable along to
    % each function that kff calls as well (the functions below use 'a').
    kffsim([0, 300], dt, x_hat_0, P_0, u, options, ...
           'UserVars', {a});
	snapnow();
    
    % Run a Monte-Carlo test with 100 simulations for 300 applications
    % each.
    kffmct(100, [0 300], dt, x_hat_0, P_0, u, options, 'UserVars', {a});
 
end % hw2ans1
 
% Update the state from sample k-1 to sample k. This function is discrete
% and doesn't actually use the time inputs.
function x_k = propagate(t_km1, t_k, x_km1, u_km1, a) %#ok
 
    p   = a * u_km1/(sum(x_km1) + 3);       % Generalized pressure
    x_k = x_km1 - p * x_km1 ./ (x_km1 + 1); % Vectorized calculation
 
end % propagate
 
% Calculate the Jacobian of the propagation function wrt the state.
function F = propagation_jacobian(t_km1, t_k, x, u, a) %#ok
 
    % Calculate the generalized pressure term.
    p = a * u/(sum(x) + 3);
    
    % Differentiate the pressure wrt x1, x2, or x3 (it's the same
    % regardless of which element of x is used).
    dpdxi = -p/(sum(x) + 3);
    
    % Differentiate the first element of the propagation function, f, by x2
    % or x3; it will be the same for either. Repeat for f2 wrt x1 or x3 and
    % f3 wrt x1 or x2.
    df1dxi = -dpdxi * x(1)/(x(1) + 1);
    df2dxi = -dpdxi * x(2)/(x(2) + 1);
    df3dxi = -dpdxi * x(3)/(x(3) + 1);
    
    % Differentiate f1 wrt x1. Part of this is the same as df1/dx2 or 
    % df1/dx3, plus a 1 and a pressure term.
    df1dx1 = 1 + df1dxi - p / (x(1) + 1)^2;
    df2dx2 = 1 + df2dxi - p / (x(2) + 1)^2;
    df3dx3 = 1 + df3dxi - p / (x(3) + 1)^2;
    
    % Build the final Jacobian.
    F = [df1dx1, df1dxi, df1dxi; ...
         df2dxi, df2dx2, df2dxi; ...
         df3dxi, df3dxi, df3dx3];
 
end % propagation_jacobian
 
% Calculate the process noise. When there's more build-up, there's more
% process noise, as described in hw2.m.
function Q = process_noise(t_km1, t_k, x, u, a) %#ok
 
    Q = diag(1e-4 + 1e-2 * x.^2);
 
end % process_noise
 
% Create the observation vector based on the current state and input.
function z = observe(t, x, u, a) %#ok
 
    % Generalized pressure
    p = a * u/(sum(x) + 3);
    
    % Generalized pressure maps the state to the measurement directly.
    z = p * x;
 
end % observe
 
% Create the Jacobian of the observation function.
function H = observation_jacobian(t, x, u, a) %#ok
 
    H = a*u/(sum(x) + 3)^2 * ...
            [x(2)+x(3)+3,       -x(1),       -x(1); ...
                   -x(2), x(1)+x(3)+3,       -x(2); ...
                   -x(3),       -x(3), x(1)+x(2)+3];
 
end % observation_jacobian

ml2jade1Z2Xml2jade1A6XNEES: Percent of data in theoretical 95.0% bounds: 75.7%
NMEE: Percent of data in theoretical 95.0% bounds: 93.6%
NIS:  Percent of data in theoretical 95.0% bounds: 97.0%
TAC:  Percent of data in theoretical 95.0% bounds: 95.1%
AC:   Percent of data in theoretical 95.0% bounds: 94.9%