From: Jim Pike 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;