[Bug 598] New: Measurement update in EKF does not use information from prediction step

Submitted by wmeeusse on Thu, 2008-11-06 23:10

BFL-dev

For more infomation about this bug, visit
Summary: Measurement update in EKF does not use information from
prediction step
Product: BFL
Version: trunk
Platform: All
OS/Version: All
Status: NEW
Severity: blocker
Priority: P2
Component: core
AssignedTo: bfl [..] ...
ReportedBy: wim [dot] meeussen [..] ...
CC: bfl [..] ...
Estimated Hours: 0.0

After a day of despair, lots of new gray hair, and fingernails for lunch, I
think I found something... strange.

In the measurement update of the EKF, the following equation is calculated:

x_k = x_k- + K(z_k - H.x_k-)

The current implementation uses the predicted state for the first x_k- after
the "=", and it uses the prior state for the second x_k- after the "=".
Shouldn't this be the predicted state in both cases? It gave me very strange
results.... I hope I'm forgetting something, otherwise this would mean we have
had funny filter results for the last six years!

Wim

The following patch makes my filter run a lot better:

Index: src/filter/extendedkalmanfilter.cpp
===================================================================
--- src/filter/extendedkalmanfilter.cpp (revision 29726)
+++ src/filter/extendedkalmanfilter.cpp (working copy)
@@ -90,6 +90,7 @@
{
// allocate measurement for z.rows() if needed
AllocateMeasModelExt(z.rows());
+ _x = _post->ExpectedValueGet();

(_mapMeasUpdateVariablesExt_it->second)._Z =
((AnalyticMeas*)measmodel)->PredictionGet(s,_x);
(_mapMeasUpdateVariablesExt_it->second)._H =
((AnalyticMeas*)measmodel)->df_dxGet(s,_x);

[Bug 598] Measurement update in EKF does not use information fro

Submitted by Tinne De Laet on Thu, 2008-12-18 10:15.

For more infomation about this bug, visit

Tinne De Laet <tinne [dot] delaet [..] ...> changed:

--- Comment #7 from Tinne De Laet <tinne [dot] delaet [..] ...> 2008-12-18 10:56:03 ---
svn commit -m "Applied patch (attachement #360): Add unit test to prevent bug
like bug#598 (Measurement update in EKF does not use information from
prediction step) "
Sending tests/CMakeLists.txt
Adding tests/ekf_test.cpp
Adding tests/ekf_test.hpp
Transmitting file data ...
Committed revision 29806.

[Bug 598] Measurement update in EKF does not use information fro

Submitted by Wim Meeussen on Thu, 2008-12-18 03:05.

For more infomation about this bug, visit

--- Comment #6 from Wim Meeussen <meeussen [..] ...> 2008-12-18 04:02:04 ---
Created an attachment (id=360)
--> (https://www.fmtc.be/bugzilla/orocos/attachment.cgi?id=360)
patch

This patch adds a cpp unit test which catches a bug like 598.

[Bug 598] Measurement update in EKF does not use information fro

Submitted by klaas on Tue, 2008-12-09 19:50.

For more infomation about this bug, visit

Klaas Gadeyne <klaas [dot] gadeyne [..] ...> changed:

--- Comment #5 from Klaas Gadeyne <klaas [dot] gadeyne [..] ...> 2008-12-09 20:27:20 ---
(In reply to comment #4)
> > > You are right.
> > > Fortunately this mistake has not been there for six years as suggested. It
> > > entered in the code while trying to avoid memory allocations during runtime.
> > > Your patch fixes the issue.
>
> > Could/Should we have detected this regression via a unit test?
>
> Could yes, and should yes. I'll put that on my todo list.

Thx, I'll reopen this one then!

[Bug 598] Measurement update in EKF does not use information fro

Submitted by wmeeusse on Tue, 2008-12-09 19:25.

For more infomation about this bug, visit

--- Comment #4 from Wim Meeussen <wim [dot] meeussen [..] ...> 2008-12-09 18:05:52 ---
> > You are right.
> > Fortunately this mistake has not been there for six years as suggested. It
> > entered in the code while trying to avoid memory allocations during runtime.
> > Your patch fixes the issue.

> Could/Should we have detected this regression via a unit test?

Could yes, and should yes. I'll put that on my todo list.

[Bug 598] Measurement update in EKF does not use information fro

Submitted by klaas on Tue, 2008-12-02 18:55.

For more infomation about this bug, visit

--- Comment #3 from Klaas Gadeyne <klaas [dot] gadeyne [..] ...> 2008-12-02 19:32:45 ---
(In reply to comment #1)
> You are right.
> Fortunately this mistake has not been there for six years as suggested. It
> entered in the code while trying to avoid memory allocations during runtime.
> Your patch fixes the issue.

Could/Should we have detected this regression via a unit test?

[Bug 598] Measurement update in EKF does not use information fro

Submitted by Tinne De Laet on Tue, 2008-12-02 08:40.

For more infomation about this bug, visit

Tinne De Laet <tinne [dot] delaet [..] ...> changed:

--- Comment #2 from Tinne De Laet <tinne [dot] delaet [..] ...> 2008-12-02 09:20:48 ---
I applied the patch Wim suggested:

svn commit extendedkalmanfilter.cpp
Sending extendedkalmanfilter.cpp
Transmitting file data .
Committed revision 29762.

[Bug 598] Measurement update in EKF does not use information fro

Submitted by Tinne De Laet on Tue, 2008-12-02 08:35.

For more infomation about this bug, visit

Tinne De Laet <tinne [dot] delaet [..] ...> changed:

--- Comment #1 from Tinne De Laet <tinne [dot] delaet [..] ...> 2008-12-02 09:13:12 ---
You are right.
Fortunately this mistake has not been there for six years as suggested. It
entered in the code while trying to avoid memory allocations during runtime.
Your patch fixes the issue.