mems.m

Kalman filter that can be simulated under windws

% ========================== MEMS & Goniometer EKF========================
%Wei Tan, Jan, 2004
%=========================================================================
close all; clear optpar;
xfunc = 'memsstate';      % File containing state equations
yfunc = 'memsobs';     % File containing output equations
linfunc = 'memslin';   % File containing the linearization
P0= diag([1e-4 1e-4 1e-4 1e-6 1e-6 1e-6]); % Initial covariance
Q = diag([1/12 1/12]);         % Cov. of process noise
R = diag([1e-7 1e-7 1e-8]);    % Cov. of measurement noise

%----- Initializations -----
load signal.dat                % Load observations from mems experiment
x0 = [x10 x20 x30 x40 x50]';   % Initial state = first observation

u = 0;
[xhat,Pmat]=ekf(xfunc,yfunc,linfunc,x0,P0,Q,R,u,y,tidx);

%----- Display the results -----
samples=size(u,1);
minval = min([xhat(:,[1:3]);y]);
maxval = max([xhat(:,[1:3]);y]);
figure(1)
subplot(311)
plot(0:samples,xhat(:,1),'-',tidx,y(:,1),'+')
grid
axis([0 samples minval(1) maxval(1)])
ylabel('x-coordinate')

subplot(312)
plot(0:samples,xhat(:,2),'-',tidx,y(:,2),'+')
grid
axis([0 samples minval(2) maxval(2)])
ylabel('y-coordinate')

subplot(313)
plot(0:samples,xhat(:,3),'-',tidx,y(:,3),'+')
grid
axis([0 samples minval(3) maxval(3)])
ylabel('theta')
xlabel('Time (samples)')

figure(2)
clf
subplot(211)
plot(0:samples,xhat(:,4:5))
grid
axis([0 samples min(min(xhat(:,4:5))) max(max(xhat(:,4:5)))])
ylabel('Right and left wheel radius')

subplot(212)
plot(0:samples,xhat(:,6))
grid
axis([0 samples min(xhat(:,6)) max(xhat(:,6))])
ylabel('Wheel distance')
xlabel('Time (samples)')