systemconstr.m

kalman filter update equations implemented in this code

function [EstErrors, ConstrErrors] = SystemConstr(nMonte, DisplayFlag, CorrectQFlag)

% Kalman filtering for a constrained vehicle tracking problem
% INPUTS:   nMonte = # of Monte Carlo simulations
%           DisplayFlag = flag indicating whether or not to display results
%           CorrectQFlag = flag indicating whether or not to use correct Q and P0 for filters
% OUTPUTS:  EstErrors = nMonte x 6 array containing estimation errors
%           ConstrErrors = nMonte x 6 array containing constraint errors
%               1st col = results for unconstrained filter
%               2nd col = results for perfect measurement filter
%               3rd col = results for estimate projection filter
%               4th col = results for moving horizon estimator
%               5th col = results for system projection filter
%               6th col = results for pdf truncaction filter

if ~exist('CorrectQFlag', 'var')
    CorrectQFlag = false;
end
if ~exist('nMonte', 'var')
    nMonte = 1;
end
if ~exist('DisplayFlag', 'var')
    DisplayFlag = true;
end

tf = 150; % final time (seconds)
T = 3; % time step (seconds)
horizon = 4; % MHE horizon size

theta = pi / 3; % heading angle (measured CCW from east)
tantheta = tan(theta);

% System matrices.
F = [1 0 T 0;
    0 1 0 T;
    0 0 1 0;
    0 0 0 1];
G = [0; 0; T*sin(theta); T*cos(theta)];
H = [1 0 0 0;
    0 1 0 0];

% State constraint: Dx = d
D = [1 -tantheta 0 0;
    0 0 1 -tantheta];
d = [0; 0];

%D = [0 0 1 -tantheta];
%d = 0;

% Note the following constraint does not work because it says that
% the position is constrained while the velocity is not constrained, but
% it is the velocity that determines the position (through the state equations).
%D = [1 -tantheta 0 0];
%d = 0;

Q = diag([4, 4, 1, 1]); % Process noise covariance (m, m, m/sec, m/sec)
R = diag([900, 900]); % Measurement noise covariance (m, m)

% Define the initial state and the initial estimation error covariance
x0 = [0; 0; tantheta; 1] * 100;
P0 = diag([R(1,1), R(2,2), Q(1,1), Q(2,2)]);

for i = 1 : nMonte
    disp(['Simulation # ', num2str(i)]);
    [EstErrors(i,:), ConstrErrors(i,:)] = ...
        KalmanConstr(tf, T, horizon, F, G, H, D, d, Q, R, x0, P0, DisplayFlag, CorrectQFlag);
end
disp('RMS Estimation Errors (unconstr, perfect meas, est proj, MHE, sys proj, pdf trunc):');
disp(mean(EstErrors, 1));
disp('RMS Constraint Errors (unconstr, perfect meas, est proj, MHE, sys proj, pdf trunc):');
disp(mean(ConstrErrors, 1));