Skip to content

A very simple example applying Kalman Filter

31/05/2011

Problem: Consider a very simple example:
y_k=x+v_k
where x is a deterministic unknown value. Here x has the real value as 1. y_k is a measurement. Our goal is to estimate the x using measurement y_k.

This problem is very simple. It may arise when we try to meansure the length of an object. Or we can treat the measurements as a noisy signal.

State Space Model: In order to apply Kalman Filter. Rewrite it to state space model.
x_{k}=1\cdot x_{k-1}+0
y_k=1\cdot x_{k}+v_i
where E(v_iv_j)=\delta_{ij}R.

Apply Kalman Filter: then we have
P_{k,k-1}=P_{k-1}+0
K_k=P_{k,k-1}(P_{k,k-1}+R)^{-1}
\hat{x}_{k,k-1}=\hat{x}_{k-1}
\hat{x}_k=\hat{x}_{k,k-1}+K_k(z_k-\hat{x}_{k,k-1})
P_k=(1-K_k)P_{k,k-1}(1-K_k)+K_kRK_k
Choose initial values of \hat{x}_0, P_0, R as -1, 2, 3.

Matlab Code:

clc;clear;close all
num=100;
x=ones(1,num); % real value
z=x;
for i=1:num
    z(i)=x(i)+randn(1); % measurement
end
x_hat=sum(z)/num;

figure;
subplot(2,1,1);hold on;grid on
title 'estimation of x_k';
subplot(2,1,2); hold on; grid on
title 'estimation of P_k';
% initial value
x_k_1=-1;
P_k_1=2;
subplot(2,1,1);
plot(0,x_k_1, 'r>');
subplot(2,1,2);
plot(0,P_k_1, 'o');
R=3;
for k=1:num
    z_k=z(k);

    %P_k_k_1=P_k_1 % 1
    K_k=P_k_1*inv(P_k_1+R); % 2
    % x_k_k_1=x_k_1 % 3
    x_k=x_k_1+K_k*(z_k-x_k_1); % 4
    P_k=(1-K_k)*P_k_1*(1-K_k)'+K_k*R*K_k'; % 5

    subplot(2,1,1)
    plot(k,x_k,'r>');
    plot(k,z_k,'g*');
    subplot(2,1,2)
    plot(k,P_k,'o');

    % for next loop
    P_k_1=P_k;
    x_k_1=x_k;
end


Simulation results:


Advertisements
No comments yet

Leave a Reply

Please log in using one of these methods to post your comment:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s