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$
5
  • $\begingroup$ what does your process noise covariance matrix look like? $\endgroup$ Jul 24, 2015 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, 2015 at 20:03
  • $\begingroup$ @DavidWurtz: Thanks for your comment! I added the process noise covariance matrix to my question above. $\endgroup$
    – Janis
    Jul 27, 2015 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, 2015 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$ Jul 27, 2015 at 19:53

1 Answer 1

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$

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