# Wrong estimation of derivatives with an extended Kalman filter

I am trying to implement an extended Kalman filter (EKF) in MATLAB for the estimation of joint trajectories (angular position, angular velocity and angular acceleration) from noisy motion capture measurements using a constant angular jerk process model. This is my process model:

$x_{prior}^k = \begin{bmatrix} 1 & \Delta t & \frac{\Delta t^2}{2} & \frac{\Delta t^3}{6} \\ 0 & 1 & \Delta t & \frac{\Delta t^2}{2} \\ 0 & 0 & 1 & \Delta t \\ 0 & 0 & 0 & 1 \end{bmatrix} \cdot x_{postiror}^{k-1}$

and process noise covariance:

$Q = 2 \cdot \sigma_p^2 \cdot \begin{bmatrix} \frac{\Delta t^7}{252} & \frac{\Delta t^6}{72} & \frac{\Delta t^5}{30} & \frac{\Delta t^4}{24} \\ \frac{\Delta t^6}{72} & \frac{\Delta t^5}{20} & \frac{\Delta t^4}{8} & \frac{\Delta t^3}{6} \\ \frac{\Delta t^5}{30} & \frac{\Delta t^4}{8} & \frac{\Delta t^3}{3} & \frac{\Delta t^2}{2} \\ \frac{\Delta t^4}{24} & \frac{\Delta t^3}{6} & \frac{\Delta t^2}{2} & \Delta t \end{bmatrix}$

My estimates for the angular positions look ok, but my estiamtes for the derivatives are quite wrong. Here is a graph from a simple example application with a comparisson between the angular velocity estimated by my implementation (EKF) and a quite noisy central difference (CD).

Somehow, I cannot improve the estimation of the derivatives by tuning the covariances, initial values or (almost) anything else. So I think there must be something wrong with my implementation. But I cannot find anything, maybe I suffer from tunnel vision...

I adopted the EKF equations from the paper by Yu et al. 2.

% Apply extended Kalman filter equations
for k = 1:T

% Apply time update
if k == 1

% Equation (15) in [Yu2004]
X_prior(k, :) = f(x0);
A = dfdx(x0);
W = dfdw(x0);

% Equation (12) in [Yu2004]
P_prior(:, :, k) = A * P0 * A' + W * Q * W';

else

% Equation (15) in [Yu2004]
X_prior(k, :) = f(X_posterior((k - 1), :)');
A = dfdx(X_posterior((k - 1), :)');
W = dfdw(X_posterior((k - 1), :)');

% Equation (12) in [Yu2004]
P_prior(:, :, k) = A * P_prior(:, :, (k - 1)) * A' + W * Q * W';

end

% Apply measurement update
H = dhdx(X_prior(k, :)');
V = dhdv(X_prior(k, :)');

% Equation (11) in [Yu2004] using the pseudo inverse to handle
K = P_prior(:, :, k) * H' / (V * R * V' + H * P_prior(:, :, k) * H');

% Equation (14) in [Yu2004]
X_posterior(k, :) = X_prior(k, :) + (K * (Z(k, :)' - h(X_prior(k, :)')))';

% Equation (10) in [Yu2004]
P_posterior(:, :, k) = P_prior(:, :, k) - K * H * P_prior(:, :, k);

end


There seems to be one difference in the computation of the prior error covariance compared to the equations given in the paper by Welch and Bishop 3.

Yu et al.:

$P_{prior}^k = A \cdot P_{prior}^{k - 1} \cdot A^T + W \cdot Q \cdot W^T$

Welch and Bishop:

$P_{prior}^k = A \cdot P_{posterior}^{k - 1} \cdot A^T + W \cdot Q \cdot W^T$

The equation by Yu et al. produces the wrong derivatives and the equation by Welch and Bishop seems to result in instable estimations.

I compiled a simple example application with my implementation 4. It should work out of the box, if you run "TestKalmanFilter.m" in MATLAB and you should see the given graph and a small animation of a double pendulum.

Any help is greatly appreciated. Janis

• what does your process noise covariance matrix look like? – David Wurtz Jul 24 '15 at 16:51
• One thing I've never been sure about. People commonly use the kinematic model, regardless of the actual physics involved as the KF model. So what does this mismatch in modeling lead to? Could that explain the issue you see? – docscience Jul 24 '15 at 20:03
• @DavidWurtz: Thanks for your comment! I added the process noise covariance matrix to my question above. – Janis Jul 27 '15 at 11:02
• @docscience: Thanks for your comment! Indeed, there is a systematic mismatch, yes. But the angular position is estimated quite well, so in my understanding the derivatives should work too. – Janis Jul 27 '15 at 11:02
• The way your Q matrix is structured it is saying that uncertainty enters the process at every state. You might want to try zeroing the whole Q matrix except for the bottom right value. By doing that you're saying that uncertainty only enters the system through the angular jerk, and then diffuses to the rest of the states. – David Wurtz Jul 27 '15 at 19:53

Finally, I found my mistake. It was a combination of unfavorable parameters and the wrong computation of the prior error covariance. Yu et al. and Welch and Bishop actually give the same equation. I just got confused by the indices used by Yu et al. So the correct listing for the EKF should be:

% Apply extended Kalman filter equations
for k = 1:T

% Apply time update
if k == 1

% Equation (15) in [Yu2004]
X_prior(k, :) = f(x0);
A = dfdx(x0);
W = dfdw(x0);

% Equation (12) in [Yu2004]
P_prior(:, :, k) = A * P0 * A' + W * Q * W';

else

% Equation (15) in [Yu2004]
X_prior(k, :) = f(X_posterior((k - 1), :)');
A = dfdx(X_posterior((k - 1), :)');
W = dfdw(X_posterior((k - 1), :)');

% Equation (12) in [Yu2004]
P_prior(:, :, k) = A * P_posterior(:, :, (k - 1)) * A' + W * Q * W';

end

% Apply measurement update
H = dhdx(X_prior(k, :)');
V = dhdv(X_prior(k, :)');

% Equation (11) in [Yu2004] using the pseudo inverse to handle
K = P_prior(:, :, k) * H' / (V * R * V' + H * P_prior(:, :, k) * H');

% Equation (14) in [Yu2004]
X_posterior(k, :) = X_prior(k, :) + (K * (Z(k, :)' - h(X_prior(k, :)')))';

% Equation (10) in [Yu2004]
P_posterior(:, :, k) = P_prior(:, :, k) - K * H * P_prior(:, :, k);

end