local position acceleration: use mean value between two publications #24105
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
To avoid aliasing on the ned acceleration, add an accumulation of acceleration to improve the downsampling
|
For navigation you could also rather use |
bresch
requested changes
Dec 13, 2024
| lpos.ax = vel_deriv(0); | ||
| lpos.ay = vel_deriv(1); | ||
| lpos.az = vel_deriv(2); | ||
| if (vel_integ_duration_ > 0.001f) { |
There was a problem hiding this comment.
Suggested change
| if (vel_integ_duration_ > 0.001f) { | |
| if (vel_integ_duration_ > FLT_EPSILON) { |
| @@ -280,6 +280,9 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem | |||
| perf_counter_t _ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": EKF update")}; | |||
| perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")}; | |||
|
|
|||
| Vector3f delta_vel_{0.f, 0.f, 0.f}; | |||
| float vel_integ_duration_{0}; | |||
There was a problem hiding this comment.
Suggested change
| float vel_integ_duration_{0}; | |
| float _delta_vel_dt{0}; |
| @@ -280,6 +280,9 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem | |||
| perf_counter_t _ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": EKF update")}; | |||
| perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")}; | |||
|
|
|||
| Vector3f delta_vel_{0.f, 0.f, 0.f}; | |||
There was a problem hiding this comment.
Suggested change
| Vector3f delta_vel_{0.f, 0.f, 0.f}; | |
| Vector3f _delta_vel{0.f, 0.f, 0.f}; |
dagar
reviewed
Dec 14, 2024
| @@ -731,6 +731,10 @@ void EKF2::Run() | |||
| _ekf.setIMUData(imu_sample_new); | |||
| PublishAttitude(now); // publish attitude immediately (uses quaternion from output predictor) | |||
|
|
|||
| // accumulate delta velocity to compute mean acceleration | |||
| delta_vel_ += _ekf.getVelocityDerivative() * imu_sample_new.delta_vel_dt; | |||
There was a problem hiding this comment.
We could do this directly in the output predictor so that getVelocityDerivative() returns the average.
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Solved Problem
On our onboard computer, we use the vehicle local position message for estimation and control purpose.
We use the acceleration fields to have a higher order than just velocity and position.
We have observed aliasing on tests with vibrations. The NED acceleration integral become very different from the velocity.
Solution
To avoid aliasing on the NED acceleration, add an accumulation of acceleration to improve the down sampling.
This implementation looks like a hack... Of course, I'm open to any suggestions for a better implementation.
Test coverage