eskf with sensor concept, less states, and latest fixes from dev/auv-navigation-eskf

[AhmedBorch]

Hi, I did the following modifications:

• added the sensor concept requirements.
• removed gravity from the states because it doesn't change and computation would be faster without it -> reduced matrices sizes
• add the modifications from Talha's branch: naming convention, latest fixes, use of utils.
• changed the params (IMU is not rotated in the simulation)
• I had to add a .tpp file for the function template
• I used NDEBUG for publishing NIS, so it will only be published when building with debug flag.

btw I didn't branch from Talha's because I had already made lots of modifications, it was easier to implement the latest missing fixes from his branch in here, I hope this is okay:)

P.S:

• R is 0 (maybe I should add some values in the simulation)
• I will make a separate file to calculate errors and nees
• the S is very big in the beginning but then gets smaller, I will need to investigate this more (maybe have a big initial covariance)

[Andeshog]

Your branch is behind the main branch, and that is causing merge conflicts. Update that and make sure all tests pass 😄 Also, you could still change the target branch to Talha's branch so the PR can be continued there, but it probably doesnt matter if this is up to date.

[codecov]

Codecov Report

❌ Patch coverage is 1.34228% with 441 lines in your changes missing coverage. Please review.
✅ Project coverage is 36.80%. Comparing base (f4f793e) to head (7e13a67).
⚠️ Report is 1 commits behind head on main.

Files with missing lines	Patch %	Lines
navigation/eskf/src/eskf_ros.cpp	0.00%	258 Missing ⚠️
navigation/eskf/src/eskf.cpp	0.00%	99 Missing ⚠️
...ission/landmark_server/src/landmark_server_ros.cpp	0.00%	30 Missing ⚠️
...ce_filter_dp_quat/src/ros/reference_filter_ros.cpp	0.00%	15 Missing ⚠️
navigation/eskf/include/eskf/eskf.tpp	0.00%	14 Missing ⚠️
...vigation/odom_transformer/src/odom_transformer.cpp	0.00%	11 Missing ⚠️
...sion/waypoint_manager/src/waypoint_manager_ros.cpp	42.85%	8 Missing ⚠️
navigation/eskf/include/eskf/eskf.hpp	0.00%	2 Missing ⚠️
navigation/eskf/include/eskf/eskf_ros.hpp	0.00%	2 Missing ⚠️
navigation/eskf/include/eskf/typedefs.hpp	0.00%	2 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             main     #640      +/-   ##
==========================================
- Coverage   38.26%   36.80%   -1.47%     
==========================================
  Files          83       85       +2     
  Lines        6687     6966     +279     
  Branches     2481     2528      +47     
==========================================
+ Hits         2559     2564       +5     
- Misses       3450     3724     +274     
  Partials      678      678              

Flag	Coverage Δ
unittests	36.80% <1.34%> (-1.47%)	⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...er/include/landmark_server/landmark_server_ros.hpp	0.00% <ø> (ø)
...rmer/include/odom_transformer/odom_transformer.hpp	0.00% <ø> (ø)
navigation/eskf/include/eskf/eskf.hpp	0.00% <0.00%> (ø)
navigation/eskf/include/eskf/eskf_ros.hpp	0.00% <0.00%> (ø)
navigation/eskf/include/eskf/typedefs.hpp	0.00% <0.00%> (ø)
...sion/waypoint_manager/src/waypoint_manager_ros.cpp	46.66% <42.85%> (-0.73%)	⬇️
...vigation/odom_transformer/src/odom_transformer.cpp	0.00% <0.00%> (ø)
navigation/eskf/include/eskf/eskf.tpp	0.00% <0.00%> (ø)
...ce_filter_dp_quat/src/ros/reference_filter_ros.cpp	0.00% <0.00%> (ø)
...ission/landmark_server/src/landmark_server_ros.cpp	0.00% <0.00%> (ø)
... and 2 more

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[chrstrom]

?

[AhmedBorch]

removed

[chrstrom]

if you're looking for quick/easy performance gains, i think you can just use cholesky in here instead of doing a full inversion (should also constrain you to PSD-ness). Won't be anything major as the DVL measurement updates are very slow and small dims, but good for the future if higher-rate sensors are ever used.

[AhmedBorch]

fixed

[chrstrom]

You sure you want to do this? Not having true-north I'd think fixing the orientation of the odom frame as "whatever it is on startup" is better than offsetting by -90deg?

[AhmedBorch]

The -90deg was left there because the auv is initially rotated by that in the simulation (drone_orientation: "-1.57 0.0 0.0")
So on sim we get big errors because of that. This will be fixed soon I've heard.
I removed it.

[chrstrom]

have you looked into the runtime performance of this one here? If its an issue you could speed it up quite a bit using a "good-enough" truncated taylor. If its already "good enough" then no need to change it 🔥

(For reference, did some very primitive testing on this, and keeping terms up to dt^3 is a 5x speedup while keeping errors vs the full exponentiation very low)

[AhmedBorch]

Currently it takes about 12-15 us with the .exp()
It is 5 times faster with Taylor expansion, but that only saves about 10us
However, Taylor expansion might have a higher error in case of a lag/jitter of the imu (so higher dt). I think this should be checked further when testing on the real hardware.