Kalman Filter - Velocity [Matlab]
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 &lt;- 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 &lt;- 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 &lt;- 128;
  dt &lt;- 0.01

  t &lt;- seq(0,T-1)*dt

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

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

  true_acceleration &lt;- rnorm(T,0,1)
  measurement &lt;- t(matrix(rep(0,T*2),c(2,T)))
  x &lt;- t(matrix(rep(0,T*3),c(3,T)))

  position_noise &lt;- rnorm(T,0,0.001)
  velocity_noise &lt;- rnorm(T,0,20)

  v &lt;- t(array(c(position_noise, velocity_noise),c(T,2)))

  Q &lt;- matrix(c(0,0,0,0,0,0,0,0,1),3,3)
  R &lt;- 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 &lt;- matrix(rep(0,2*T),c(T,2))

  for (k in 1:T)
  {
    xkkm1 &lt;- A %*% xkm1km1
    Pkkm1 &lt;- A %*% Pkm1km1 %*% t(A) + Q
    K &lt;- Pkkm1 %*% t(H) %*% ginv( H %*% Pkkm1 %*% t(H) + R)
    z &lt;- matrix(c(measurement[k,]), 2, 1)
    xkm1km1 &lt;- xkkm1 + K %*% (z - H %*% xkkm1)
    Pkm1km1 &lt;- (matrix(c(1,0,0,0,1,0,0,0,1),3,3) - K %*% H) %*% Pkkm1  
    zhat[k,] &lt;- 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 &lt;- c(sum((signal - zhat)^2), sum((signal - lpf)^2))
  print(errs)