kalmanconstr.m

kalman filter update equations implemented in this code

function [EstErrors, ConstrErrors] = ...
    KalmanConstr(tf, T, horizon, A, B, H, D, d, Q, R, x, P, DisplayFlag, CorrectQFlag)

% This m-file simulates various Kalman filters.
% This routine is called by SystemConstr.m.
% For details about the algorithms, see http://academic.csuohio.edu/simond/ConstrKF.
% INPUTS:   tf = final time
%           T = step size
%           horizon = moving horizon estimator (MHE) horizon size
%           A = state transition matrix
%           B = input matrix
%           H = measurement matrix
%           D = constraint matrix (Dx=d)
%           d = constraint vector
%           Q = process noise covariance
%           R = measurement noise covariance
%           x = initial state
%           P = initial state estimate covariance
%           DisplayFlag = flag indicating whether to display and plot results
%           CorrectQFlag = flag indicating whether or not to use correct Q and P0 for filters
% OUTPUTS:  EstErrors = Array of RMS estimation errors. This is an array containing results for:
%               (1) The unconstrained Kalman filter
%               (2) The perfect measurement filter
%               (3) The estimate projection filter (W=P^{-1})
%               (4) The moving horizon estimator
%               (5) The system projection filter
%               (6) The pdf truncation filter
%           ConstrErrors = Array of RMS constraint errors.

n = size(Q, 1); % number of states
Rsqrt = sqrt(R);
xhat = x; % Unconstrained Kalman filter
xhat1 = x; % Kalman filter with perfect measurements
xtilde = x; % Constrained Kalman filter (estimate projection, W=inv(P))
xhatSys = x; % Constrained Kalman filter (system projection)
xhatMHE = x; % Constrained Kalman filter (moving horizon estimation)
xTrunc = x; % Constrained Kalman filter (pdf truncation)

% Measurement matrix for perfect measurement filter
H1 = [H; D];
% Measurement noise covariance for perfect measurement filter
R1 = R;
for i = 1 : length(d)
    R1(end+1, end+1) = 0;
end

% Initial estimation error covariance for constrained Kalman filter (system projection)
[u, s, v] = svd(D');
r = length(d); % number of constraints
u2 = u(:, r+1:end);
PND = u2 * u2';
Pc = PND * P * PND;
% Process noise covariance for constrained Kalman filter (system projection).
% Note that this is the *real* process noise covariance.
Qc = PND * Q * PND;

[dQc, lambdaQc, dQcT] = svd(Qc);
for i = 1 : size(lambdaQc, 1)
    if real(lambdaQc(i,i)) < 0
        % This should never be true, but it may occur because of numerical issues.
        lambdaQc(i,i) = 0;
    end
end
ddT = dQc * dQc';
if (norm(eye(size(ddT)) - ddT) > 1e-8)
    disp('Error - dQc is not orthogonal.');
    return;
end
if (norm(dQc*lambdaQc*dQc' - Qc) > 1e-8)
    disp('Error - SVD failed for Qc');
    return;
end

if CorrectQFlag
    Q = Qc;
    P = Pc;
end

% Initial estimation error covariance for perfect measurement formulation
P1 = P;

% moving horizon estimation initialization
Parray(:, :, 1) = P;
P1array(:, :, 1) = P1;
xhatMHE0 = xhat;
zMHE = [];

% Initialize arrays for saving data for plotting.
xarray = x;
xhatarray = xhat;
Constrarray = D * xhat;
xhat1array = xhat1;
Constr1array = D * xhat1;
xtildearray = xtilde;
ConstrTildearray = D * xtilde;
xhatSysarray = xhatSys;
ConstrSysarray = D * xhatSys;
xhatMHEarray = xhatMHE;
ConstrMHEarray = D * xhatMHE;
xTruncArray = xTrunc;
ConstrTruncArray = D * xTrunc;

randn('state', sum(100*clock));
In = eye(n, n);

% Begin the simulation.
for t = T : T : tf
    u = 0; % If this is changed from zero then MHE needs to be modified
    % Simulate the system.
    x = A * x + B * u + dQc * sqrt(lambdaQc) * randn(size(x));
    z = H * x + Rsqrt * randn(size(H,1), 1);
    % Run the Kalman filter.
    P = A * P * A' + Q;
    headinghat = atan2(xhat(3), xhat(4));
    B = [0; 0; T*sin(headinghat); T*cos(headinghat)];
    xhat = A * xhat + B * u;
    K = P * H' * inv(H * P * H' + R);
    xhat = xhat + K * (z - H * xhat);
    P = (In - K * H) * P;
    % Find the constrained (estimate projection) Kalman filter estimates.
    xtilde = xhat - P * D' * inv(D*P*D') * D * xhat;
    % Run the constrained Kalman filter (system projection).
    Pc = A * Pc * A' + Qc;
    headinghatSys = atan2(xhatSys(3), xhatSys(4));
    B = [0; 0; T*sin(headinghatSys); T*cos(headinghatSys)];
    xhatSys = A * xhatSys + B * u;
    Kc = Pc * H' * inv(H * Pc * H' + R);
    xhatSys = xhatSys + Kc * (z - H * xhatSys);
    Pc = (In - Kc * H) * Pc;
    % Run the filter for the perfect measurement formulation.
    z1 = [z; d];
    P1 = A * P1 * A' + Q;
    headinghat1 = atan2(xhat1(3), xhat1(4));
    B1 = [0; 0; T*sin(headinghat1); T*cos(headinghat1)];
    xhat1 = A * xhat1 + B1 * u;
    S = H1 * P1 * H1' + R1;
    if rank(S) < length(z1)
        S = S + 1e-8;
    end
    K1 = P1 * H1' * inv(S);
    xhat1 = xhat1 + K1 * (z1 - H1 * xhat1);
    P1 = (In - K1 * H1) * P1;
    % Moving horizon estimation for linear system
    zMHE = [zMHE z];
    if size(zMHE, 2) > horizon
        zMHE = zMHE(:, 2:end);
        lngthxhatMHEarray = size(xhatMHEarray, 2);
        xhatMHE0 = xhatMHEarray(:, lngthxhatMHEarray-horizon+1);
    end
    PMHE0 = Parray(:, :, 1);
    lngthP = size(Parray, 3) + 1;
    Parray(:, :, lngthP) = P;
    if lngthP > horizon
        Parray = Parray(:, :, 2:end);
    end
%     PMHE0 = P1array(:, :, 1);
%     lngthP = size(P1array, 3) + 1;
%     P1array(:, :, lngthP) = P1;
%     if lngthP > horizon
%         P1array = P1array(:, :, 2:end);
%     end
    xhatMHE = MHE(PMHE0, xhatMHE0, A, H, Q, R, zMHE, D, d);
    % Constrained filtering via probability density function truncation
    PTrunc = P;
    xTrunc = xhat;
    for k = 1 : r
        [Utrunc, Wtrunc, Vtrunc] = svd(PTrunc);
        Ttrunc = Utrunc;
        TTT = Ttrunc * Ttrunc';
        if (norm(eye(size(TTT)) - TTT) > 1e-8)
           disp('Error - Ttrunc is not orthogonal.');
           return;
        end
        if (norm(Utrunc*Wtrunc*Utrunc' - PTrunc) > 1e-8)
            disp('Error - SVD failed for pdf trunction');
            return;
        end
        % Compute the modified Gram-Schmidt transformation S * Amgs = [ Wmgs ; 0 ].
        % Amgs is a given n x m matrix, and S is an orthogonal n x n matrix, and Wmgs is an m x m matrix.
        Amgs = sqrt(Wtrunc) * Ttrunc' * D(k,:)'; % n x 1, where n = number of states
        [Wmgs, S] = MGS(Amgs);
        S = S * sqrt(D(k,:) * PTrunc * D(k,:)') / Wmgs;
        cTrunc = (d(k) - D(k,:) * xTrunc) / sqrt(D(k,:) * PTrunc * D(k,:)');
        dTrunc = (d(k) - D(k,:) * xTrunc) / sqrt(D(k,:) * PTrunc * D(k,:)');
        % The next 3 lines are for inequality constraints. In our example, they
        % are commented out because our problem uses equality constraints.
        %alpha = sqrt(2/pi) / erf(dTrunc/sqrt(2)) - erf(cTrunc/sqrt(2));
        %mu = alpha * (exp(-cTrunc^2/2) - exp(-dTrunc^2/2));
        %sigma2 = alpha * (exp(-cTrunc^2/2) * (cTrunc - 2 * mu) - exp(-dTrunc^2/2) * (dTrunc - 2 * mu)) + mu^2 + 1;
        
        % The following two lines are used for equality constraints.
        % Since we have equality constraints, the mean of zTrunc = cTrunc = dTrunc,
        % and its variance is 0.
        mu = cTrunc;
        sigma2 = 0;
        
        zTrunc = zeros(size(xTrunc));
        zTrunc(1) = mu;
        CovZ = eye(length(zTrunc));
        CovZ(1,1) = sigma2;
        xTrunc = Ttrunc * sqrt(Wtrunc) * S' * zTrunc + xTrunc;
        PTrunc = Ttrunc * sqrt(Wtrunc) * S' * CovZ * S * sqrt(Wtrunc) * Ttrunc';
    end
    % Compute the constraint errors
    ConstrErr = D * xhat - d;
    ConstrTilde = D * xtilde - d;
    Constr1Err = D * xhat1 - d;
    ConstrMHEErr = D * xhatMHE - d;
    ConstrSysErr = D * xhatSys - d;
    ConstrTruncErr = D * xTrunc - d;
    % Save data in arrays
    xarray = [xarray x];
    xhatarray = [xhatarray xhat];
    xtildearray = [xtildearray xtilde];
    xhat1array = [xhat1array xhat1];
    xhatMHEarray = [xhatMHEarray xhatMHE];    
    xhatSysarray = [xhatSysarray xhatSys]; 
    xTruncArray = [xTruncArray xTrunc];
    Constrarray = [Constrarray ConstrErr]; 
    ConstrTildearray = [ConstrTildearray ConstrTilde];
    Constr1array = [Constr1array Constr1Err];
    ConstrMHEarray = [ConstrMHEarray ConstrMHEErr];
    ConstrSysarray = [ConstrSysarray ConstrSysErr];
    ConstrTruncArray = [ConstrTruncArray ConstrTruncErr];
end % end simulation loop
% Compute RMS estimation errors - note we are computing the RMS estimation errors
% only of the first 2 states.
EstError = xarray - xhatarray;
EstError = mean(EstError(1,:).^2 + EstError(2,:).^2);
EstError = sqrt(EstError);
EstError1 = xarray - xhat1array;
EstError1 = mean(EstError1(1,:).^2 + EstError1(2,:).^2);
EstError1 = sqrt(EstError1);
EstErrorTilde = xarray - xtildearray;
EstErrorTilde = mean(EstErrorTilde(1,:).^2 + EstErrorTilde(2,:).^2);
EstErrorTilde = sqrt(EstErrorTilde);
EstErrorMHE = xarray - xhatMHEarray;
EstErrorMHE = mean(EstErrorMHE(1,:).^2 + EstErrorMHE(2,:).^2);
EstErrorMHE = sqrt(EstErrorMHE);
EstErrorSys = xarray - xhatSysarray;
EstErrorSys = mean(EstErrorSys(1,:).^2 + EstErrorSys(2,:).^2);
EstErrorSys = sqrt(EstErrorSys);
EstErrorTrunc = xarray - xTruncArray;
EstErrorTrunc = mean(EstErrorTrunc(1,:).^2 + EstErrorTrunc(2,:).^2);
EstErrorTrunc = sqrt(EstErrorTrunc);
% Compute constraint errors
r = length(d); % number of constraints
Constr = 0; for i = 1 : r, Constr = Constr + Constrarray(i,:).^2; end
Constr = sqrt(mean(Constr));
Constr1 = 0; for i = 1 : r, Constr1 = Constr1 + Constr1array(i,:).^2; end
Constr1 = sqrt(mean(Constr1));
ConstrTilde = 0; for i = 1 : r, ConstrTilde = ConstrTilde + ConstrTildearray(i,:).^2; end
ConstrTilde = sqrt(mean(ConstrTilde));
ConstrMHE = 0; for i = 1 : r, ConstrMHE = ConstrMHE + ConstrMHEarray(i,:).^2; end
ConstrMHE = sqrt(mean(ConstrMHE));
ConstrSys = 0; for i = 1 : r, ConstrSys = ConstrSys + ConstrSysarray(i,:).^2; end
ConstrSys = sqrt(mean(ConstrSys));
ConstrTrunc = 0; for i = 1 : r, ConstrTrunc = ConstrTrunc + ConstrTruncArray(i,:).^2; end
ConstrTrunc = sqrt(mean(ConstrTrunc));
EstErrors = [EstError, EstError1, EstErrorTilde, EstErrorMHE, EstErrorSys, EstErrorTrunc];
ConstrErrors = [Constr, Constr1, ConstrTilde, ConstrMHE, ConstrSys, ConstrTrunc];
if DisplayFlag
    close all; t = 0 : T : tf;
    figure;
    plot(t, xarray(1, :), ':', t, xarray(2, :), '-');
    title('True Position');
    xlabel('seconds'); ylabel('meters'); legend('north', 'east');
    figure;
    plot(t, xarray(1, :) - xhatarray(1, :), ':', t, xarray(2, :) - xhatarray(2, :), '-');
    title('Position Estimation Error (Unconstrained)');
    xlabel('seconds'); ylabel('meters'); legend('north', 'east');
    figure;
    plot(t, xarray(1, :) - xhat1array(1, :), ':', t, xarray(2, :) - xhat1array(2, :), '-');
    title('Position Estimation Error (Perfect Measurements)');
    xlabel('seconds'); ylabel('meters'); legend('north', 'east');
    figure;
    plot(t, xarray(1, :) - xtildearray(1, :), ':', t, xarray(2, :) - xtildearray(2, :), '-');
    title('Position Estimation Error (Estimate Projection, W=inv(P))');
    xlabel('seconds'); ylabel('meters'); legend('north', 'east');
    figure;
    plot(t, xarray(1, :) - xhatMHEarray(1, :), ':', t, xarray(2, :) - xhatMHEarray(2, :), '-');
    title('Position Estimation Error (MHE)');
    xlabel('seconds'); ylabel('meters'); legend('north', 'east');
    figure;
    plot(t, xarray(1, :) - xhatSysarray(1, :), ':', t, xarray(2, :) - xhatSysarray(2, :), '-');
    title('Position Estimation Error (System Projection)');
    xlabel('seconds'); ylabel('meters'); legend('north', 'east');
    figure;
    plot(t, xarray(1, :) - xTruncArray(1, :), ':', t, xarray(2, :) - xTruncArray(2, :), '-');
    title('Position Estimation Error (pdf Truncation)');
    xlabel('seconds'); ylabel('meters'); legend('north', 'east');
end