Kalman filter, car tracking, Matlab

Question

I have a small data set of a moving car:

Data_=[time x,y,z];    %# ONLY THIS DATA

I know that in this case velocity and acceleration are not constant.

I want to estimate the car position at various times. I decided to use Kalman filter. I searched for Kalman filter but I couldn't find code for tracking an object in 3D space with velocity and acceleration. I don't know where to start. Can Kalman filter automatically handle velocity and acceleration?

Can some one help me and give some link or some guidance?

Answer 1

My recommendation is to go to the Mathworks file exchange and search for Kalman filters

You'll find several good pieces of code for this very standard algorithm.

As far as Kalman filters themselves, they are what's called a Predictor-Estimator. That is, they can do prediction of the state at time n given the observations up to time n-1. Then after you've received the observations at time n you can do estimation (some call it smoothing) for all times up to and including time n. The estimation part is done through what's called an innovation and through the current Kalman gain.

Kalman filters work through the concept of a "state space", that is your state stores all the necessary information about the object. The observation vector, which are different, are what you can observe about your system. In a constant acceleration model, for example, you'll probably assume that the state only contains the 3 position values and the 3 velocity values (x, y, and z for each). It's the designer of the filter's job to decide the state space and the state transition model (how you expect the state to change in absence of observations.)

You'll have to choose a state transition matrix, you'll have to have some knowledge of the covariance matrix of the error of your observations, of the covariance matrix of the error in your state transition matrix (i.e., how good your state transition model is), and the covariance matrix of your initial state estimate (which you have to also choose). You'll also have to choose the relationship between the state vector and the observation vector.

Kalman filters are the maximum likelihood optimal linear estimator if you assume Gaussian observation noise, Gaussian process noise and a few other standard things.

Answer 2

Kalman Filter is 5-6 lines in a loop. You do not need anybody's implementation.

What you need is a linear system model that describes the trajectory of your car. If you have the system matrices A,B,C (or F,G,H) you are practically done.

Kalman Filter is a general Bayesian filtering algorithm. It will work for any linear gaussian case.

Answer 3

Check out the udacity.com course on AI ,CS373. There they have explained kalman filter very nicely.

Answer 4

The Computer Vision System Toolbox now has a vision.KalmanFilter object. Here is an example of how to use it for tracking objects. The example is in 2D, but it can be easily generalized to 3D.

Answer 5

The (linear) Kalman filter is probably the best option in your case (one of its first applications was in fact to track the position of the Apollo space ship to properly hit the moon!). So there are plenty of tutorials on exactly this problem, e.g. see the example with this cute little robot. And it is in fact some 5 lines of code (note that you should use persistent variables). The tuning of the covariance matrices (often P, R, Q) is an educated guessing. Initialize P as a diagonal matrix P = eye(length(x))*1e3 and choose the noise matrices R, Q roughly in the same order of the state vector x or the measurements y respectively.

If you don't like the fuzzy with noise matrices, you can use a recursive fit: RLS (recursive-least-squares) is a standard identification method -- but it does not use any statistics as the Kalman filter, i.e. it is prone to noise in the measurements. It consists of even less lines of code but also uses persistent variables.

function [x_est] = RLS(y,x0,mk,fnc)
% (non)linear recursive-least-square
%
% y     measurements
% x0    initial value of the to-be-identified state vector
% mk    measurement matrix, so that y = mk'*x
% fnc   function handle, if the system is non-linear and when mk is the
%       linearized version of this function

persistent  x P 
if isempty(x)

    x = x0;
    P = eye(length(x)) * 1e3;
end


% adaption factor (usually [0.9 1))
AdaptFct = .995;


%% nonlinear prediction
if nargin > 4
    y_sim = fnc(x);
else
    y_sim = mk*x;
end
e = y_sim - y;


%% RLS
% Kalman gain
G = P*mk / (AdaptFct + mk'*P*mk);

% Update
% state
x = x + G*e;
% covarianve matrix
I = eye(length(P));
P_new = (I - G*mk')*P;
P = P_new/AdaptFct;

%% output
x_est = x;

end

Note that you have to clear the persistent variables of a function if you want to restart everything: clear RLS