schmidt.m

《卡曼滤波基础-------实用方法》

function Schmidt

% Reduced order Kalman filter using consider states.

phix = 1; phiy = 1;
phi = [phix 0; 0 phiy];
Qx = 1; Qy = 0;
Q = [Qx 0; 0 Qy];
Hx = 1; Hy = 1;
H = [Hx Hy];
R = 1;
Pxminus = 1.6; Pxyminus = 0; Pyxminus = Pxyminus'; Pyminus = 1;
Pminus = [Pxminus Pxyminus; Pyxminus Pyminus];

x = [0; 0];
xhatminus = [0; 0];
xhatminusSchmidt = [0];

xhatErr = [];
xhatErrSchmidt = [];

tf = 20;
for t = 0 : tf
    % Simulate the measurement
    z = H * x + sqrt(R) * randn;
    % Simulate the full order filter
    K = Pminus * H' * inv(H * Pminus * H' + R);
    xhatplus = xhatminus + K * (z - H * xhatminus);
    Pplus = (eye(2) - K * H) * Pminus * (eye(2) - K * H)' + K * R * K';
    xhatminus = phi * xhatplus;
    Pminus = phi * Pplus * phi' + Q;
    % Simulate the Kalman-Schmidt filter
    alpha = Hx * Pxminus * Hx' + Hx * Pxyminus * Hy' + Hy * Pxyminus * Hx' + Hy * Pyminus * Hy' + R;
    Kx = (Pxminus * Hx' + Pxyminus * Hy') * inv(alpha);
    xhatplusSchmidt = xhatminusSchmidt + Kx * (z - Hx * xhatminusSchmidt);
    Pxplus = (eye(1) - Kx * Hx) * Pxminus - Kx * Hy * Pyxminus;
    Pxyplus = (eye(1) - Kx * Hx) * Pxyminus - Kx * Hy * Pyminus;
    Pyxplus = Pxyplus';
    Pyplus = Pyminus;
    xhatminusSchmidt = phix * xhatplusSchmidt;
    Pxminus = phix * Pxplus * phix' + Qx;
    Pxyminus = phix * Pxyplus * phiy';
    Pyxminus = Pxyminus';
    Pyminus = phiy * Pyplus * phiy' + Qy;    
    % Save data for later
    xhatErr = [xhatErr; x(1) - xhatplus(1)];
    xhatErrSchmidt = [xhatErrSchmidt; x(1) - xhatplusSchmidt];
    % Simulate the state dynamics
    x = phi * x + [Qx * randn; Qy * randn];
end

t = 0 : tf;
close all;
plot(t, xhatErr(1:21), 'r-', t, xhatErrSchmidt(1:21), 'b--');
set(gca,'FontSize',12); set(gcf,'Color','White');
legend('Full Order Filter', 'Reduced Order Filter');
xlabel('Time Step'); ylabel('Estimation Error');

xhatErr = std(xhatErr);
xhatErrSchmidt = std(xhatErrSchmidt);
disp(['RMS Error = ', num2str(xhatErr), ' (full order filter), ', num2str(xhatErrSchmidt), ' (Schmidt filter)']);