Moves calculation of innovation from KalmanFilter to AnalyticMeas.Mdl.G.U.

[toeklk]

migrated from Bugzilla #780
status NEW severity normal in component core for ---
Reported in version trunk on platform All
Assigned to: BFL mailinglist

Original attachment names and IDs:

• ekf_meas_update_use_innovation.patch (ID 612)

On 2010-09-14 18:34:18 +0200, Thimo Langbehn wrote:

  Created attachment 612 Patches InnovationGet for trunk rev. 32851 This patch changes the KalmanFilter, ExtendedKalmanFilter and AnalyticMeasurementModelGaussianUncertainty classes of the Bayesian Filtering Library (BFL). The calculation of the innovation is moved from the class KalmanFilter to the class MeasurementModel and the existing functions for KalmanFilter and ExtendedKalmanFilter are adapted without changing the signature and therefore without the need to change other code. Rationale --------- In robotic applications it is often necessary to use bearing measurements from sensors for some tasks. If a bearing based measurement model is used in conjunction with the ExtendedKalmanFilter, the Innovation may contain angular difference values, which have a cyclic value range (2PI = 0). Since those values are represented as double, they need to be normalized to avoid incorrect results. However, with the current implementation of the KalmanFilter it is not possible to normalize the Innovation, since it is calculated inside the class KalmanFilter, and the KalmanFilter cannot know which of the vector elements are such cyclic values since it depends on the used measurement model. Solution -------- The calculation of the innovation is moved to the class AnalyticMeasurementModelGaussianUncertainty, to a new interface function named InnovationGet, and thus can be overwritten by a specific derivation, if needed. Otherwise the default implementation is used which calculates the innovation by simple subtraction, as it was done before. The implementation of KalmanFilter::CalculateMeasUpdate(z,Z,...) was moved to a second CalculateMeasUpdate(zZ,...) function that takes the innovation (zZ) as an argument insted of the z and Z parameters. The implementation was adapted to use the provided innovation, of course. The existing CalculateMeasUpdate(z,Z,...) version was changed to call the new one with the difference z-Z (hardcoded), since KalmanFilter has no way of accessing the used measurement model in that function. Implications for depended code ------------------------------ Existing derivations of the KalmanFilter class may be adapted, but it is not required. The way this change is implemented leads to exactly the same behavior as before for the existing CalculateMeasUpdate function, even if the used derivation of AnalyticMeasurementModelGaussianUncertainty does overwrite the InnovationGet function. Implementations of classes derived from KalmanFilter may be gradually changed to the new, extracted approach. Then the existing CalculateMeasUpdate(z,Z,...) function could be deprecated and removed to get rid of the second locations the innovation is calculated in. Author ------ Patch written by Thimo Langbehn for The Orocos Project: http://orocos.org