Attitude Estimator Numerical Stability

[kylekrol]

In short, we've observed some numerical stability issues with the cxx attitude filter implementation. Below are listed some potential trouble spots and any observations/alternate implementations that have been explored.

Calculation of x_bar and z_bar

There isn't a ton to say here other than in the initial attitude filter implementation switching between the below methods of calculation x_bar and z_bar results in very different values when using floats.

The following:

x_bar = weight_c * state.sigmas[0];
z_bar = weight_c * state.measures[0];
for (lin::size_t i = 1; i < 13; i++) {
  x_bar = x_bar + weight_o * state.sigmas[i];
  z_bar = z_bar + weight_o * state.measures[i];
}

x_bar = weight_c * state.sigmas[0];
z_bar = weight_c * state.measures[0];
for (lin::size_t i = 1; i < 7; i++) {
  x_bar = x_bar + weight_o * (state.sigmas[i] + state.sigmas[i+6]);
  z_bar = z_bar + weight_o * (state.measures[i] + state.measures[i+6]);
}

I think this stems from the fact that in this particular testcase the sigma points 6 away from each other tend to be the negative of their counterpart. This can result in catastrophic cancellation when all components aren't of a similar magnitude.

Kalman Gain Calculation

The Kalman gain K is defined as being equal to P_xy * inv(P_vv) where inv denotes the matrix inverse. The straightforward implementation is to calculate inv(P_vv) directly as shown below:

UkfMatrix6x5 K;
{
  UkfMatrix5x5 Q, R;
  lin::qr(state.P_vv, Q, R);
  lin::backward_sub(R, Q, lin::transpose(Q).eval());
  K = state.P_xy * Q;
}

However, it's possible to leverage the fact that P_vv is a covariance matrix and therefore should be symmetric. It's possible to write P_vv * transpose(K) = transpose(P_XY) which then let's us determine K as shown below:

UkfMatrix6x5 K;
{
  UkfMatrix5x6 M, N;
  UkfMatrix5x5 L = state.P_vv;
  lin::chol(L);
  lin::forward_sub(L, M, lin::transpose(state.P_xy).eval());
  lin::backward_sub(lin::transpose(L).eval(), N, M);
  K = lin::transpose(N);
}

which should be more stable. As of the initial filter implementation, however, no noticeably different in testcase output results from switching between the two methods above.