ekf2: cleanup output predictor init #20960
Open
+982
−800
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
Member
dagar
commented
Jan 18, 2023
dagar
commented
- output predictor align also init _output_vert_new
- output predictor reset handle all fields
- output predictor resetQuaternion() normalize
- output predictor calculateOutputStates() paranoid timestamp check
- output predictor correctOutputStates() paranoid timestamp checks and explicitly pop delayed sample at correct time
dagar
force-pushed
the
pr-ekf2_output_predictor_init_cleanup
branch
5 times, most recently
from
6bbfef5 to
b4a313d
Compare
dagar
force-pushed
the
pr-ekf2_output_predictor_init_cleanup
branch
from
b4a313d to
284073f
Compare
dagar
force-pushed
the
pr-ekf2_output_predictor_init_cleanup
branch
3 times, most recently
from
e9003b7 to
a5efedd
Compare
dagar
force-pushed
the
pr-ekf2_output_predictor_init_cleanup
branch
from
a5efedd to
c32a89e
Compare
bresch
requested changes
Apr 25, 2023
- output predictor align also init _output_vert_new - output predictor reset handle all fields - output predictor resetQuaternion() normalize - output predictor calculateOutputStates() paranoid timestamp check - output predictor correctOutputStates() paranoid timestamp checks and explicitly pop delayed sample at correct time
dagar
force-pushed
the
pr-ekf2_output_predictor_init_cleanup
branch
from
c32a89e to
b172705
Compare
bresch
reviewed
Sep 21, 2023
bresch
reviewed
Oct 25, 2023
dagar
force-pushed
the
pr-ekf2_output_predictor_init_cleanup
branch
from
edb8750 to
0290a61
Compare
bresch
requested changes
Oct 27, 2023
Comment on lines
+352
to
+374
| // calculate the quaternion rotation delta from the EKF to output observer states at the EKF fusion time horizon | ||
| const Quatf q_delta((quat_state * output_delayed.quat_nominal.inversed()).normalized()); | ||
| const Dcmf rot_change{q_delta}; | ||
|
|
||
| // add the reset amount to the output observer buffered data | ||
| for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { | ||
|
|
||
| _output_buffer[i].quat_nominal = (q_delta * _output_buffer[i].quat_nominal).normalized(); | ||
|
|
||
| // apply change to velocity | ||
| _output_buffer[i].vel = rot_change * _output_buffer[i].vel; | ||
| _output_buffer[i].vel_alternative = rot_change * _output_buffer[i].vel_alternative; | ||
| } | ||
|
|
||
| // apply the change in attitude quaternion to our newest quaternion estimate | ||
| _output_new.quat_nominal = (q_delta * _output_new.quat_nominal).normalized(); | ||
|
|
||
| // calculate a gain that provides tight tracking of the estimator attitude states and | ||
| // adjust for changes in time delay to maintain consistent damping ratio of ~0.7 | ||
| const uint64_t time_latest_us = _time_last_update_states_us; | ||
| const float time_delay = fmaxf((time_latest_us - time_delayed_us) * 1e-6f, _dt_update_states_avg); | ||
| const float att_gain = 0.5f * _dt_update_states_avg / time_delay; | ||
|
|
||
| // calculate a corrrection to the delta angle | ||
| // that will cause the INS to track the EKF quaternions | ||
| _delta_angle_corr = delta_ang_error * att_gain; | ||
| _output_tracking_error(0) = delta_ang_error.norm(); | ||
| // apply change to velocity | ||
| _output_new.vel = rot_change * _output_new.vel; | ||
| _output_new.vel_alternative = rot_change * _output_new.vel_alternative; | ||
|
|
||
| /* | ||
| * Loop through the output filter state history and apply the corrections to the velocity and position states. | ||
| * This method is too expensive to use for the attitude states due to the quaternion operations required | ||
| * but because it eliminates the time delay in the 'correction loop' it allows higher tracking gains | ||
| * to be used and reduces tracking error relative to EKF states. | ||
| */ | ||
| propagateVelocityUpdateToPosition(); |
There was a problem hiding this comment.
Isn't this exactly the same as in resetQuaternion?
|
|
||
| // apply change to velocity | ||
| _output_buffer[i].vel = rot_change * _output_buffer[i].vel; | ||
| _output_buffer[i].vel_alternative = rot_change * _output_buffer[i].vel_alternative; |
There was a problem hiding this comment.
I see what you're trying to do here, but why correcting the velocities here if they are anyway re-corrected just after?
| const float vert_vel_err = (vel_state(2) - output_vert_delayed.vert_vel); | ||
| const float vert_vel_integ_err = (pos_state(2) - output_vert_delayed.vert_vel_integ); | ||
| // calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon | ||
| output_delayed.vel = q_delta.rotateVectorInverse(output_delayed.vel); |
There was a problem hiding this comment.
Shouldn't we use rotateVector(... or rot_change * ... here?
Also, it seems that this is only needed because the velocity is corrected during the quaternion correction above.
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.