3
$\begingroup$

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).

Difference between filter estimate (EKF) and central difference estimate (CD) of the first derivative.

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
% badly scaled matrices 
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

$\endgroup$
  • $\begingroup$ what does your process noise covariance matrix look like? $\endgroup$ – David Wurtz Jul 24 '15 at 16:51
  • $\begingroup$ 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? $\endgroup$ – docscience Jul 24 '15 at 20:03
  • $\begingroup$ @DavidWurtz: Thanks for your comment! I added the process noise covariance matrix to my question above. $\endgroup$ – Janis Jul 27 '15 at 11:02
  • $\begingroup$ @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. $\endgroup$ – Janis Jul 27 '15 at 11:02
  • $\begingroup$ 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. $\endgroup$ – David Wurtz Jul 27 '15 at 19:53
0
$\begingroup$

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
% badly scaled matrices 
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

Thank you for your comments and votes!

$\endgroup$

protected by jojek Sep 22 '16 at 17:34

Thank you for your interest in this question. Because it has attracted low-quality or spam answers that had to be removed, posting an answer now requires 10 reputation on this site (the association bonus does not count).

Would you like to answer one of these unanswered questions instead?

Not the answer you're looking for? Browse other questions tagged or ask your own question.