https://www.fmtc.be/bugzilla/orocos/show_bug.cgi?id=780
Summary: Moves calculation of innovation from KalmanFilter to
AnalyticMeas.Mdl.G.U.
Product: BFL
Version: trunk
Platform: All
OS/Version: All
Status: NEW
Severity: normal
Priority: P3
Component: core
AssignedTo: bfl [..] ...
ReportedBy: thimo [..] ...
CC: thimo [..] ..., bfl [..] ...
Estimated Hours: 0.0
Created an attachment (id=612)
--> (https://www.fmtc.be/bugzilla/orocos/attachment.cgi?id=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 <thimo [..] ...>
for The Orocos Project: http://orocos.org