0
$\begingroup$

There are several algorithms regarding fixed-lag Kalman smoothing. In most cases, an augmented state vector is defined in which the elements are the current and delays of the original state vector.

However, in some cases the measurements are augmented to include delays, as in: https://pdfs.semanticscholar.org/f580/23aa0827e4a5ab1d04957b2b6cf266aad541.pdf

And in some cases, only the k'th measurement is used in the augmented system, as in section 3.2.2 of: http://robby.caltech.edu/~jwb/courses/CDS112/FixedLagSmoother.pdf

I ran a simulation which yielded better results when augmenting the measurements vector, but as far as I understood, replicating the measurements is a simple linear transformation of the measurements which should not improve the performance of the augmented filter. Is there a theoretical justification for augmenting the measurements vector?

Matlab simulation code:

% model: 
% x(k+1) = F*x(k) + w(k)
% y(k)   = H*x(k) + v(k)
% R = cov(v), Q = cov(w)

% generate clean data
T = 100;
dt = 1e-2;
t = 0:dt:T;
v0 = [40;50];
r0 = [0;0];
a = [0;-9.81];

v = v0 + a*t; % constant acceleration
r = r0 + cumsum(v*dt,2);

inds = find(r(2,:) > 0);
N = numel(inds);
t = t(inds);
r = r(:,inds);
v = v(:,inds);
a = repmat(a, [1 N]);

% generate measurements
rng(0);
sig_r = 3*1e0;
rm = r + sig_r*randn(2,N);

% kalman filter
x = zeros(6,1); % position, velocity, acceleration
P = 10*eye(6,6);

% acceleration is known...
P(5,5) = 0.5;
P(6,6) = 0.5;

F = [eye(2)   eye(2)*dt eye(2)*(dt^2)/2; ...
     zeros(2) eye(2)    eye(2)*dt; ...
     zeros(2) zeros(2)  eye(2)];

% position measurement only
H = [1 0 zeros(1,4); ...
     0 1 zeros(1,4)];

R = eye(2)*sig_r^2;
vQ = [dt^3/6;dt^3/6;dt^2/2;dt^2/2;dt;dt];
sig_j = 1e0;
Q = vQ*vQ'*sig_j^2;

rf = zeros(size(rm));
xf = zeros(6,N);
Pf = zeros(6,6,N);
Pf_pred_inv = zeros(6,6,N);
Pf_pred = zeros(6,6,N);

D = 20;
Fa = zeros(6*(D+1));
Fa(1:6,1:6) = F;
Fa(7:end,1:end-6) = eye(6*D);
xa = zeros(6*(D+1),1);
Pa = zeros(numel(xa));
Pa(1:6,1:6) = P;
Qa = zeros(size(Pa));
Qa(1:6,1:6) = Q;

% single measurement
xA = xa;
PA = Pa;
Ra = R;
Ha = [H zeros(size(H,1),size(H,2)*D)];

% augmented measurements
RA = kron(eye(D+1),R);
HA = kron(eye(D+1),H); % D+1 replicas

for k = 1:N
    % Kalman filter
    % predict
    P_pred = F*P*F' + Q;
    xA_pred = F*x;

    Pf_pred(:,:,k) = P_pred;
    Pf_pred_inv(:,:,k) = inv(P_pred);

    % update
    y = rm(:,k) - H*xA_pred;
    S = R + H*P_pred*H';
    Sinv = inv(S);
    K = P_pred*H'*Sinv;
    x = xA_pred + K*y;
    P = (eye(6) - K*H)*P_pred;

    xf(:,k) = x;
    Pf(:,:,k) = P;
    rf(:,k) = x(1:2);    


    % kalman smoother (augmented measurements)
    if(k+D > N)
        v_rA(:,k) = rf(:,k);
    else
        % augmented measurements
        rA = rm(:,k+D:-1:k);
        rA = rA(:);

        % predict
        PA_pred = Fa*PA*Fa' + Qa;
        xA_pred = Fa*xA;

        % update
        yA = rA - HA*xA_pred;
        SA = RA + HA*PA*HA';
        KA = PA_pred*HA'*inv(SA);
        xA = xA_pred + KA*yA;
        PA = (eye(6*(D+1)) - KA*HA)*PA_pred;

        v_rA(:,k) = xA(end-5:end-4); % last position in buffer
    end

    % kalman smoother (single measurement)
    if(k+D > N)
        v_ra(:,k) = rf(:,k);
    else
        % augmented measurements
        ra = rm(:,k+D);

        % predict
        Pa_pred = Fa*Pa*Fa' + Qa;
        xa_pred = Fa*xa;

        % update
        ya = ra - Ha*xa_pred;
        Sa = Ra + Ha*Pa*Ha';
        Ka = Pa_pred*Ha'*inv(Sa);
        xa = xa_pred + Ka*ya;
        Pa = (eye(6*(D+1)) - Ka*Ha)*Pa_pred;

        v_ra(:,k) = xa(end-5:end-4); % last position in buffer
    end

end

% position
figure(1);
plot(r(1,:), r(2,:), '.b');
hold on;
plot(rm(1,:), rm(2,:), '.r');
plot(rf(1,:), rf(2,:), '.k');
plot(v_rA(1,:), v_rA(2,:), '.c');
plot(v_ra(1,:), v_ra(2,:), '.g');

legend('true position', 'measured', 'filtered', 'smoothed, augmented measurement', 'smoothed, single measurement');

% error
figure(2);
Ef = sqrt(sum((r-rf).^2));
EA = sqrt(sum((r-v_rA).^2));
Ea = sqrt(sum((r-v_ra).^2));
plot(Ef, '.k');
hold on;
plot(EA, '.c');
plot(Ea, '.g');

legend('filter', 'smoother: augmented measurements', 'smoother: single measurement');
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.