From: Jim Pike <jpike@nyx.net>
Subject: Re: Kalman Filter
Date: Mon, 31 Jan 2000 03:20:41 GMT
Newsgroups: sci.math
Summary: [missing]

"William D. Allen Sr." wrote:
> 
> Does anyone know of an inexpensive Kalman Filter program in Basic, C+,
> Fortran, Excel or whatever?
> 
> Thanks,
> 
> WDA
> 
> ballensr@home.com
> 
> end

Here's one for free. The problem is that it doesn't do what you want.
The reason that there are no shrink wrap Kalman Filters is that 99% of
the work is tayloring to your problem. The actual equations are straight
forward.

Jim Pike

This is a Matlab script for a Kalman Filter the estimates the state and
two derivatives using a kinematic model. This is about as basic as you
can get:


q = 1; % white noise power for how much acceleration changes
R = 10;    % variance of noise in measurements

xp = [0; 0; 0]; % don't know what initial state is
Pp = [ 100 0 0; 0 100 0; 0 0 100];


delta_time = 1.0;

H = [1 0 0]; % we observe first state
Phi = [ 1 delta_time delta_time^2/2; 0 1 delta_time; 0 0 1];
Q = q * [ delta_time^5/20 delta_time^4/8 delta_time^3/6;
	  delta_time^4/8  delta_time^3/3 delta_time^2/2;
          delta_time^3/6  delta_time^2/2 delta_time ];

meas = 20 * sin ( [0:100] * pi / 20 ) + sqrt(R) * randn(1);

%init vars that we want to plot
xf = zeros(3,size(meas,2));

Pos_std = zeros(size(meas));
Vel_std = zeros(size(meas));
Acc_std = zeros(size(meas));


for i=1:size(meas,2)

    % incorporate a measurement

    K = Pp * H' * ( H * Pp * H' + R )^(-1);
    xf(:,i) = xp + K * (meas(i) - H * xp );
    Pf = ( eye(3) - K * H ) * Pp;

    Pos_std(i) = sqrt(Pf(1,1));
    Vel_std(i) = sqrt(Pf(2,2));
    Acc_std(i) = sqrt(Pf(3,3));

    % predict the next measurement

    xp = Phi * xf(:,i);
    Pp = Phi * Pf * Phi' + Q;    

end

figure(1);
plot(meas);
hold
plot(xf(1,:),'g');
plot(Pos_std,'r');
hold off;

figure(2);
plot(xf(2,:)); % velocity
hold;
plot(Vel_std,'r');
hold off;

figure(3);
plot(xf(3,:)); % acceleration
hold;
plot(Acc_std,'r');
hold off;