Kalman filter

Go To Last Post
54 posts / 0 new

Pages

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Now its working almost correctly:


function [poshat] = kalman(time,r, v, p)

% time = time vector 
% r = radius vector input
% v = upper limit for process (velocity) noise [m/10msec]
% p = upper limit for position measurement noise [m]
time = [0; time];                       %add a new zero row 
time(2) = 1;                            %inhibit divide by zero
t = 2;                                  %time index
td = abs(time(t) - time(t-1));                                                  
M = [1 0; 0 1];                             % System model  
x = [0 0]';                                 % Initial state 
xhat = x;                                   % Initial state estimate

C = [1 0];                                  % Observation model

W = v * [td^2 td; td 1];                  % Process noise covariance ???
N = p;                                    % Measurement noise covariance 
                                          % N=0 Perfect follow

P = W;                                      % Initial estimate covariance

pos = [];                                   % Position
posmeas = [];                               % Measured position
poshat = [];                                % Estimated position 


[col row] = size (time);

for t = 2 : 1 : col,
    td = (time(t) - time(t-1));
    
   
    x = [r(t-1) (xhat(1) - r(t-1))/td]';   %place and velocity
    
    w = v * [td; 1];                  % Process noise ???
    x = M * x + w;                     % Simulate the system
    
    n = p;                             % Measurement noise
    z = C * x + n;                     % Simulate the measurement
    
    y = z - C * xhat;                              % Innovation of measurement residual  
    S = C * P * C' + N;                            % Innovation covariance
    K = M * P * C' * inv(S);                       % Optimal Kalman gain
    P = M * P * M' + W - M * P * C' * inv(S)* C * P * M';  % Estimate covariance
    
    xhat = M*xhat + K * y;                % State estimate using the Kalman filter
   
    
    % Save actual, measured and estimated positions
    pos = [pos x(1)];
    posmeas = [posmeas z];
    poshat = [poshat xhat(1)];

end
Last Edited: Wed. Apr 23, 2008 - 02:36 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi,

the mystic offset is for simulation of gyro drift in real world application.

bye

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

I corrected covariance parameters then offset disappeared.

Pages