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
% 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
