Commit 21a06dc
committed
Keep the relative formulation opt-in, and resize the test gate it exposed
Two findings from measuring rather than assuming, both of which argue against
turning this on by default.
1. It costs geo-referencing. test-navstate-odom-gnss-fusion's ENU->map rotation
error grows to 5.69 deg against that test's 5.0 deg gate, where the
absolute-only formulation stays inside it. The mechanism is plausible: that
yaw is observed by comparing GNSS motion in ENU against odometry-derived
motion in map, and asserting the dead-reckoned pose absolutely, however
weakly, is what makes it well observed. A relative-only chain with a single
anchor does not.
2. The APE gain it buys is being paid for on a configuration that is not
recommended anyway. On the seven BotanicGarden sequences this formulation is
better than the absolute-only one on 5 of 7 -- but with everything else held
fixed, fusing that platform's wheel odometry AT ALL is worse than ignoring it
on 6 of 7. Bad odometry stays bad however correctly it is weighted.
So: default off, documented as a trade with both numbers, and the guidance is to
enable it when fusing wheel odometry without estimating geo-referencing.
Separately, MAXIMUM_SE3_FINAL_ERROR moves 0.40 -> 0.45, and that one belongs to
the covariance-accumulation commit rather than to this feature. The odometry
that test simulates is noise-only -- no slip, no systematic error -- so trusting
it more than the motion model says is free accuracy there, and the old threshold
scored an over-confident implementation better than a correct one. Worst case
across the same seven cases:
0.365 original: absolute pose fused with the LATEST INCREMENT's covariance
0.419 that covariance corrected to the accumulated dead-reckoning one
0.368 ...plus a relative factor per reading (restores part of the over-trust)
0.405 ...with the single anchor that formulation actually implies
On real data the ranking inverts: the last of those is the best arm on
BotanicGarden and the first DIVERGES on 7 of 7, estimating 4.5-6.8x the true
path length. The fixture cannot discriminate a correct odometry weight from an
over-confident one, so it must not gate that axis; it still earns its place as a
regression guard on the fusion working at all, and the threshold is sized for
that. The reasoning is recorded at the constant.
colcon test: 34/34.1 parent 212ff82 commit 21a06dc
3 files changed
Lines changed: 47 additions & 2 deletions
File tree
- mola_state_estimation_smoother
- include/mola_state_estimation_smoother
- params
- tests
Lines changed: 10 additions & 0 deletions
| Original file line number | Diff line number | Diff line change | |
|---|---|---|---|
| |||
219 | 219 | | |
220 | 220 | | |
221 | 221 | | |
| 222 | + | |
| 223 | + | |
| 224 | + | |
| 225 | + | |
| 226 | + | |
| 227 | + | |
| 228 | + | |
| 229 | + | |
| 230 | + | |
| 231 | + | |
222 | 232 | | |
223 | 233 | | |
224 | 234 | | |
| |||
Lines changed: 15 additions & 1 deletion
| Original file line number | Diff line number | Diff line change | |
|---|---|---|---|
| |||
126 | 126 | | |
127 | 127 | | |
128 | 128 | | |
129 | | - | |
| 129 | + | |
| 130 | + | |
| 131 | + | |
| 132 | + | |
| 133 | + | |
| 134 | + | |
| 135 | + | |
| 136 | + | |
| 137 | + | |
| 138 | + | |
| 139 | + | |
| 140 | + | |
| 141 | + | |
| 142 | + | |
| 143 | + | |
130 | 144 | | |
131 | 145 | | |
132 | 146 | | |
| |||
Lines changed: 22 additions & 1 deletion
| Original file line number | Diff line number | Diff line change | |
|---|---|---|---|
| |||
41 | 41 | | |
42 | 42 | | |
43 | 43 | | |
44 | | - | |
| 44 | + | |
| 45 | + | |
| 46 | + | |
| 47 | + | |
| 48 | + | |
| 49 | + | |
| 50 | + | |
| 51 | + | |
| 52 | + | |
| 53 | + | |
| 54 | + | |
| 55 | + | |
| 56 | + | |
| 57 | + | |
| 58 | + | |
| 59 | + | |
| 60 | + | |
| 61 | + | |
| 62 | + | |
| 63 | + | |
| 64 | + | |
| 65 | + | |
45 | 66 | | |
46 | 67 | | |
47 | 68 | | |
| |||
0 commit comments