Kalman filter optimality

Question

I'm trying to understand Kalman filter optimality. According to wikipedia: "It is known from the theory that the Kalman filter is optimal in case that a) the model perfectly matches the real system, b) the entering noise is white and c) the covariances of the noise are exactly known.". But what does this optimality mean and how can I test it?

I found student daves example on Kalman filter link and started testing it. I did this by changing Kalman filters estimated noise parameters. I ranked the results by taking root mean square of the estimate error and expected to get the best results when noise variances matched the real noise parameters. I was wrong. Why is this?

Here is the matlab code I used in my test (modified from Student Daves example)

% written by StudentDave
%for licensing and usage questions
%email scienceguy5000 at gmail. com

%Bayesian Ninja tracking Quail using kalman filter

clear all
% define our meta-variables (i.e. how long and often we will sample)
duration = 10  %how long the Quail flies
dt = .1;  %The Ninja continuously looks for the birdy,
%but we'll assume he's just repeatedly sampling over time at a fixed interval

% Define update equations (Coefficent matrices): A physics based model for where we expect the Quail to be [state transition (state + velocity)] + [input control (acceleration)]
A = [1 dt; 0 1] ; % state transition matrix:  expected flight of the Quail (state prediction)
B = [dt^2/2; dt]; %input control matrix:  expected effect of the input accceleration on the state.
C = [1 0]; % measurement matrix: the expected measurement given the predicted state (likelihood)
%since we are only measuring position (too hard for the ninja to calculate speed), we set the velocity variable to
%zero.

% define main variables
u = 1.5; % define acceleration magnitude
Q= [0; 0]; %initized state--it has two components: [position; velocity] of the Quail
Q_estimate = Q;  %x_estimate of initial location estimation of where the Quail is (what we are updating)
QuailAccel_noise_mag =0.05; %process noise: the variability in how fast the Quail is speeding up (stdv of acceleration: meters/sec^2)
NinjaVision_noise_mag =10;  %measurement noise: How mask-blinded is the Ninja (stdv of location, in meters)
Ez = NinjaVision_noise_mag^2;% Ez convert the measurement noise (stdv) into covariance matrix
Ex = QuailAccel_noise_mag^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; % Ex convert the process noise (stdv) into covariance matrix
P = Ex; % estimate of initial Quail position variance (covariance matrix)

% initize result variables
% Initialize for speed
Q_loc = []; % ACTUAL Quail flight path
vel = []; % ACTUAL Quail velocity
Q_loc_meas = []; % Quail path that the Ninja sees


% simulate what the Ninja sees over time
figure(2);clf
figure(1);clf
for t = 0 : dt: duration
    % Generate the Quail flight
    QuailAccel_noise = QuailAccel_noise_mag * [(dt^2/2)*randn; dt*randn];
    Q= A * Q+ B * u + QuailAccel_noise;
    % Generate what the Ninja sees
    NinjaVision_noise = NinjaVision_noise_mag * randn;
    y = C * Q+ NinjaVision_noise;
    Q_loc = [Q_loc; Q(1)];
    Q_loc_meas = [Q_loc_meas; y];
    vel = [vel; Q(2)];
    %iteratively plot what the ninja sees
    plot(0:dt:t, Q_loc, '-r.')
    plot(0:dt:t, Q_loc_meas, '-k.')
    axis([0 10 -30 80])
    hold on
end

%plot theoretical path of ninja that doesn't use kalman
plot(0:dt:t, smooth(Q_loc_meas), '-g.')

%plot velocity, just to show that it's constantly increasing, due to
%constant acceleration
%figure(2);
%plot(0:dt:t, vel, '-b.')


%% Do kalman filtering
Qa = 0.5*QuailAccel_noise_mag^2: 0.05*QuailAccel_noise_mag^2: 1.5*QuailAccel_noise_mag^2;
Ez3 = 0.5*Ez: 0.05*Ez: 1.5*Ez;
rms_tot = zeros(length(Qa), length(Ez3));
rms_tot2 = zeros(length(Qa), length(Ez3));
for i = 1:length(Qa)
    Ex2=Qa(i) * [dt^4/4 dt^3/2; dt^3/2 dt^2];
    for j=1:length(Ez3)
        Ez2=Ez3(j);
        %initize estimation variables
        Q_loc_estimate = []; %  Quail position estimate
        vel_estimate = []; % Quail velocity estimate
        Q= [0; 0]; % re-initized state
        P_estimate = P;
        P_mag_estimate = [];
        predic_state = [];
        predic_var = [];
        for t = 1:length(Q_loc)
            % Predict next state of the quail with the last state and predicted motion.
            Q_estimate = A * Q_estimate + B * u;
            predic_state = [predic_state; Q_estimate(1)] ;
            %predict next covariance
            P = A * P * A' + Ex2;
            predic_var = [predic_var; P] ;
            % predicted Ninja measurem  1ent covariance
            % Kalman Gain
            K = P*C'*inv(C*P*C'+Ez2);
            % Update the state estimate.
            Q_estimate = Q_estimate + K * (Q_loc_meas(t) - C * Q_estimate);
            % update covariance estimation.
            P =  (eye(2)-K*C)*P;
            %Store for plotting
            Q_loc_estimate = [Q_loc_estimate; Q_estimate(1)];
            vel_estimate = [vel_estimate; Q_estimate(2)];
            P_mag_estimate = [P_mag_estimate; P(2,2)];
        end
        rms_tot(i,j) = rms(Q_loc-Q_loc_estimate);
    end
end
close all;
figure;
surf(Qa, Ez3, rms_tot);
ylabel('measurement variance'); xlabel('model variance');

And here is the result image: link

Why was I wrong? Why are the results not best when the estimated model is exactly the real model? Thank you in advance :)

Kalman filter optimality

Answers (1)

Related Questions