# 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

• Welcome to DSP.SE! There are several questions here that try to tackle a similar question. See, for example, this one. One quick issue I have with your code is that really, your $Q$ matrix should be $1 \times 1$, not $3 \times 3$. the way you've defined it will make it singular which will probably not do what you want. Also set $P$ to be very large and diagonal initially. For example P = 1000*eye(3);. Your velocity noise variance is very high compared with the position variance... – Peter K. Nov 2 '15 at 17:08
• Thank you for your answer. I don't understand on your link why we set Q and R like that. Why is this formule Q = Gw * Gw' * cov(a); (from Wikipedia) is wrong in my case? To get R with Matlab should I write only R = var(vk,vk') ? – alberto Nov 2 '15 at 19:08
• Sorry, I am not clear. Let's say wk = [0 0 wgn(1,1,100)] is correct to say that Q = cov(wk,wk') ? I mean is it enough to take in account that w~N(0,Q) and is it the only one possibiity ? – alberto Nov 2 '15 at 19:25
• Wikipedia's version is also fine. What you have does not match what Wikipedia has either. Give me some time and I'll try to put together an example of what I think should be happening and, therefore, what your $\mathbf{Q}$ and $\mathbf{R}$ matrices should be. – Peter K. Nov 3 '15 at 13:38

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.

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)

• Ok thank you very much :) What I hadn't understood was: - I wanted to generate a gaussian noise for v and w but the Kalman filter supposes directly that the noise is a gaussian noise - I wanted to derive the covariance. But one more time, I only have to give a number and the Kalman filter will interpret it as the covariance of a gaussian noise. – alberto Nov 3 '15 at 19:23
• This : H <- matrix(c(1,0,0,1,0,0),c(2,3)) Should be: H <- matrix(c(1,0,0,1,0,0),2,3) or it is the same thing? – desmond13 Nov 3 '16 at 8:49
• @minidiable Looks the same to me: > H <- matrix(c(1,0,0,1,0,0),c(2,3)) > H [,1] [,2] [,3] [1,] 1 0 0 [2,] 0 1 0 > H <- matrix(c(1,0,0,1,0,0),2,3) > H [,1] [,2] [,3] [1,] 1 0 0 [2,] 0 1 0 > – Peter K. Nov 3 '16 at 11:30