Kalman Filter - Velocity [Matlab]

I have a quite typical Kalman filter to design. I really read a lot of articles about the design of this filter but the performances of my filter are still quite bad.

Here is my situation. I have a quite good measurement signal of my position (let's say a very small white noise) and a pretty noisy measurement signal of my velocity (big white noise). I want to estimate a new position (which will be I guess not very different from my measurements) and my velocity as well (which will be more different)

Here is my Matlab code: (I don't want to use the Matlab Kalman function ;) )

%% Kalman Filter Design
dt = 0.01;

%% Define coefficient matrices
% x = [position;velocity;acceleration]

% True State equation 
% x = F * x(t-1) + B * u + w

F = [ 1 dt dt^2/2
      0  1 dt
      0  0 1];

B = [0
     0
     0];
% Measurement of the true state equation
% z = H * x(t) + v
H = [1 0 0

     0 1 0];

%% Define noise

%Process noise(white noise)
% w = Gw * a 
a = [0 ; 0 ; wgn(1,1,100)]; 
Gw = [0 0 0
     0 0 0
     0 0 1];


%Process noise covariance matrix
Q = Gw * Gw' * cov(a);

%Measurement noise (white noise)
v = [wgn(1,1,0.001) ; wgn(1,1,20)]; 

%Measurement noise covariance matrix
R = cov(v)* [0.5 0
              0  1];


%% Kalman Filter 
x_estimate = [0;0;0];
P = Q;

position_estimate = [];
velocity_estimate = [];
acc_estimate = [];
P_mag_estimate = [];


predic_state =[];
predic_var = [];

z = [position_meas,velocity_meas];


for t = 1:length (z)

%Predicted state estimate
x_estimate = F * x_estimate;

predic_state = [predic_state; x_estimate(1)];

%Predicted estimate covariance
P = F * P * F' + Q;
predic_var = [predic_var; P];

%Innovation covariance
S = H * P * H' + R ;

%Kalman Gain Predict measurement covariance

K = P * H' * inv(S);

% Updated  state estimate
x_estimate = x_estimate + K * (z(t) - H * x_estimate);

%Update covariance estimation
P = (eye(size(P,2)) - K * H) * P;

%Store for plotting
position_estimate = [position_estimate; x_estimate(1)];

velocity_estimate = [velocity_estimate; x_estimate(2)];
P_mag_estimate = [P_mag_estimate; P(1)];

end

My question is: how can I find Q and R? Does it depend of what I want to do?

Thanks a lot for your help

Answer

I think I've managed to implement your system, but in R rather than matlab (sorry, I have no access to matlab and scilab stopped working for me on OSX).

The implementation gives the following estimates of the position and velocity, and seems to work even with the large velocity noise.

The top plot is the true (red), noisy (black), and estimated (blue) position. The bottom plot is the true (red), noisy (black), and estimated (blue) velocity. I've zoomed in on the bottom plot to see the true and estimated values because the noise is so large.

To your questions:

My question is: how can I find Q and R?

Well, from my code Q is

  Q <- matrix(c(0,0,0,0,0,0,0,0,1),3,3)

which would be

  Q = [0,0,0; 0,0,0 ; 0,0,1];

in matlab, I think.

And R is

  R <- matrix(c(0.001,0,0,20),2,2)

which is

  R = [0.001 0; 0 20];

in matlab.

I've generated my acceleration as a vector that is T long with a variance of 1; you seem to have generated just one value for it with a variance of 100.

As for the second part of your question:

Does it depend of what I want to do?

Generally, the better the values the Kalman filter use match the "true" values, the better the Kalman filter estimates the state. I say "true" instead of true because sometimes we can't know what the truth is, so we have to guess it.

The only leeway I see in what you've presented is what the value if Q is. It's not clear to me what variance you assumed the acceleration to have. If it is a constant, then the variance will be zero; if it's a Gaussian random variable, then you'll need to set the Q matrix using the variance of that random variable.

Also, this all assumes that the process noise and the measurement noise are uncorrelated (or independent). If there is correlation, then you'll somehow need to take that into account.

---

R Code

  # 26818
  T <- 128;
  dt <- 0.01

  t <- seq(0,T-1)*dt

  xkm1km1 <- matrix(c(0, 0, 0),3,1)
  Pkm1km1 <- matrix(c(1000,0,0 ,0,1000,0, 0,0,1000),3,3)
  #H = [1 0 0
  #     0 1 0];

  H <- matrix(c(1,0,0,1,0,0),c(2,3)) 

  #F = [ 1 dt dt^2/2
  #      0  1 dt
  #      0  0 1];
  A <- t(matrix(c(1,dt,dt^2/2,0,1,dt,0,0,0),3,3))
  B <- matrix(c(0,0,1),c(3,1))

  true_acceleration <- rnorm(T,0,1)
  measurement <- t(matrix(rep(0,T*2),c(2,T)))

  x <- t(matrix(rep(0,T*3),c(3,T)))

  position_noise <- rnorm(T,0,0.001)
  velocity_noise <- rnorm(T,0,20)

  v <- t(array(c(position_noise, velocity_noise),c(T,2)))

  Q <- matrix(c(0,0,0,0,0,0,0,0,1),3,3)
  R <- matrix(c(0.001,0,0,20),2,2)



  # Generate the data using the signal model
  for (k in 2:T)
  {
    x[k,] = A %*% x[k-1,] + B * true_acceleration[k-1]
    measurement[k,] = H %*% x[k,] + v[,k]
  }

  library("MASS") # For pseudo inverse ginv()


  zhat <- matrix(rep(0,2*T),c(T,2))

  for (k in 1:T)
  {
    xkkm1 <- A %*% xkm1km1
    Pkkm1 <- A %*% Pkm1km1 %*% t(A) + Q
    K <- Pkkm1 %*% t(H) %*% ginv( H %*% Pkkm1 %*% t(H) + R)
    z <- matrix(c(measurement[k,]), 2, 1)
    xkm1km1 <- xkkm1 + K %*% (z - H %*% xkkm1)
    Pkm1km1 <- (matrix(c(1,0,0,0,1,0,0,0,1),3,3) - K %*% H) %*% Pkkm1  

    zhat[k,] <- as.numeric(H %*% xkkm1)
  }

  par(mfrow=c(2,1))
  plot(t, measurement[,1], type="b", pch=19)
  lines(t, x[,1], col="red",  lwd=10)
  lines(t, zhat[,1], col="blue", lwd=5)

  plot(t, measurement[,2], type="b", pch=19, ylim=c(-1,1))
  lines(t, x[,2], col="red",  lwd=10)

  lines(t, zhat[,2], col="blue", lwd=5)


  errs <- c(sum((signal - zhat)^2), sum((signal - lpf)^2))
  print(errs)