25 replies to this topic

[Jonathan Brandmeyer]

Quote

An interesting case is the one which sees a wrong setting of initial covariance. If the initial estimation error is very large, but the initial covariance is not increased according to it, we have the results showed [sic] in figure 16. The EKF diverges while the UKF converges [emphasis added]. The divergence of attitude angles brings also the divergence of all the other state variables.

That's easy to fix. Always initialize the filter with a high initial attitude covariance (pi^2, or maybe pi^2/4), to reflect that you don't know anything about the initial orientation. If your filter is good enough, it will always converge, even in-flight.  At least, that is the standard that I am holding myself to.

[Jonathan Brandmeyer]

Jonathan Brandmeyer, on 28 June 2010 - 04:04 PM, said:

Using rank-one updates in velocity runs a risk of filter divergence, because it introduces a large "factorization nonlinearity" when updating the quaternion-valued orientation mean.  Factorization nonlinearity is the nonlinearity that arises from considering rotations in 3D to be linearly factorizable into some set of basis vectors.  The cross-covariance between velocity and attitude is strong during filter operation, but the cross-covariance between position and attitude is weak. Therefore, you can get away with rank-one updates in position, but not in velocity.

Nope, this was flat wrong.  I had a bug in my implementation of rank-one Kalman updates.  Using rank-one updates in velocity is perfectly fine.

[Lew Payne]

Are you talking about rank-one updates to the Cholesky factorization?  So as to reduce new Cholesky computations from O(n^3) to O(n^2)?

[Jonathan Brandmeyer]

Lew Payne, on 19 July 2010 - 09:38 PM, said:

Are you talking about rank-one updates to the Cholesky factorization?  So as to reduce new Cholesky computations from O(n^3) to O(n^2)?

No, rank-one updates to the filter covariance.  If the measurement covariance is a diagonal matrix, then it reduces to a set of rank-one (scalar) measurements.  So a complete GPS position-velocity measurement drops down from a 6x6 matrix inverse to 6 scalar divisions. See also Triangular covariance factorizations for Kalman filtering.  Thornton describes them early in the dissertation.

[Lew Payne]

In case this helps, or if anyone's interested...

It looks like Jose-Luis Blanco has updated his "Derivation and Implementation of a Full 6D EKF-based Solution to Range-Bearing SLAM" paper, as of August 2010, and the updated version's about to be released by the MRPT (Mobile Robot Programming Toolkit) site...

See the 6D-SLAM page for more info (as well as a link to the new paper, when it does get released).

[Brian Gou]

A well documented extended kalman filter library based on C++, wish useful if anyone needed.

http://kalman.sourceforge.net/