Some sensors (e.g. magnetometers) have an output which is 3-dimensional, but have only two degrees of freedom since their output is expected to be of a fixed magnitude. Including these in the measurement model as regular 3-vectors doesn't work very well at all since the assumption of a Gaussian distribution fails spectacularly.
This patch adds a specialised UKF::FieldVector type which is represented internally by rotation vectors, in order to correctly calculate the mean and covariance of the measurement sigma point distribution.
One of the implications of the reduction in degrees of freedom is that the resulting innovation covariance is always rank-deficient and therefore positive semi-definite. This required some changes to the filter to allow it to be dealt with properly; these changes should hopefully also make the filter more numerically stable in general.
Some sensors (e.g. magnetometers) have an output which is 3-dimensional, but have only two degrees of freedom since their output is expected to be of a fixed magnitude. Including these in the measurement model as regular 3-vectors doesn't work very well at all since the assumption of a Gaussian distribution fails spectacularly.
This patch adds a specialised UKF::FieldVector type which is represented internally by rotation vectors, in order to correctly calculate the mean and covariance of the measurement sigma point distribution.
One of the implications of the reduction in degrees of freedom is that the resulting innovation covariance is always rank-deficient and therefore positive semi-definite. This required some changes to the filter to allow it to be dealt with properly; these changes should hopefully also make the filter more numerically stable in general.