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

Now its working almost correctly:

- Log in or register to post comments

TopHi,

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

bye

- Log in or register to post comments

TopI corrected covariance parameters then offset disappeared.

- Log in or register to post comments

Top## Pages