fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 19:19:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: fix rot vec calc from quat by using matrix lib

Browse Source
This commit is contained in:
ChristophTobler 2018-02-08 10:22:01 +01:00
parent 9238e2f1a2
commit cdc6efc5d6
1 changed files with 3 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
EKF/ekf_helper.cpp
View File

@ -1505,36 +1505,10 @@ void Ekf::calcExtVisRotMat()
q_error.normalize();
// convert to a delta angle and apply a spike and low pass filter
Vector3f rot_vec;
float delta;
float scaler;
if (q_error(0) >= 0.0f) {
delta = 2 * acosf(q_error(0));
if (delta > 1e-6f) {
scaler = 1.0f / sinf(delta/2);
} else {
// The rotation is too small to calculate a vector accurately
// Make the rotation vector zero
scaler = 0.0f;
}
} else {
delta = 2 * acosf(-q_error(0));
if (delta > 1e-6f) {
scaler = -1.0f / sinf(delta/2);
} else {
// The rotation is too small to calculate a vector accurately
// Make the rotation vector zero
scaler = 0.0f;
}
}
rot_vec(0) = q_error(2) * scaler;
rot_vec(1) = q_error(3) * scaler;
rot_vec(2) = q_error(4) * scaler;
Vector3f rot_vec = q_error.to_axis_angle();
float rot_vec_norm = rot_vec.norm();
if (rot_vec_norm > 1e-6f) {
// ensure magnitude of rotation matches the quaternion
rot_vec = rot_vec * (delta / rot_vec_norm);
// apply an input limiter to protect from spikes
Vector3f _input_delta_vec = rot_vec - _ev_rot_vec_filt;
@ -1567,23 +1541,11 @@ void Ekf::resetExtVisRotMat()
q_error.normalize();
// convert to a delta angle and reset
Vector3f rot_vec;
float delta;
if (q_error(0) >= 0.0f) {
delta = 2 * acosf(q_error(0));
rot_vec(0) = q_error(1) / sinf(delta/2);
rot_vec(1) = q_error(2) / sinf(delta/2);
rot_vec(2) = q_error(3) / sinf(delta/2);
} else {
delta = 2 * acosf(-q_error(1));
rot_vec(0) = -q_error(2) / sinf(delta/2);
rot_vec(1) = -q_error(3) / sinf(delta/2);
rot_vec(2) = -q_error(4) / sinf(delta/2);
}
Vector3f rot_vec = q_error.to_axis_angle();
float rot_vec_norm = rot_vec.norm();
if (rot_vec_norm > 1e-9f) {
_ev_rot_vec_filt = rot_vec * delta / rot_vec_norm;
_ev_rot_vec_filt = rot_vec;
} else {
_ev_rot_vec_filt.zero();
}