54 posts / 0 new

## Pages

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