dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Make use of inverse rotation function consistent with name 

The quaternion to inverse rotation matrix function has been updated so that the rotation it produces is the inverse to that produced by the matrix library and the the inverse of the quaternion is uses. This function is now used to directly calculate an inverse rotation matrix rather than calculating the forward rotation and then transposing it.
This commit is contained in:
Paul Riseborough 2019-08-22 09:30:49 +10:00 committed by Roman Bapst
parent 36de2b3dc1
commit 4d37065f1b
6 changed files with 12 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
EKF/drag_fusion.cpp
View File

@ -82,8 +82,7 @@ void Ekf::fuseDrag()
rel_wind(0) = vn - vwn;
rel_wind(1) = ve - vwe;
rel_wind(2) = vd;
Dcmf earth_to_body(_state.quat_nominal);
earth_to_body = earth_to_body.transpose();
Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
rel_wind = earth_to_body * rel_wind;
// perform sequential fusion of XY specific forces

13
EKF/ekf_helper.cpp
View File

@ -1382,6 +1382,7 @@ Vector3f EstimatorInterface::cross_product(const Vector3f &vecIn1, const Vector3
}
// calculate the inverse rotation matrix from a quaternion rotation
// this produces the inverse rotation to that produced by the math library quaternion to Dcmf operator
Matrix3f EstimatorInterface::quat_to_invrotmat(const Quatf &quat)
{
float q00 = quat(0) * quat(0);
@ -1399,12 +1400,12 @@ Matrix3f EstimatorInterface::quat_to_invrotmat(const Quatf &quat)
dcm(0, 0) = q00 + q11 - q22 - q33;
dcm(1, 1) = q00 - q11 + q22 - q33;
dcm(2, 2) = q00 - q11 - q22 + q33;
dcm(0, 1) = 2.0f * (q12 - q03);
dcm(0, 2) = 2.0f * (q13 + q02);
dcm(1, 0) = 2.0f * (q12 + q03);
dcm(1, 2) = 2.0f * (q23 - q01);
dcm(2, 0) = 2.0f * (q13 - q02);
dcm(2, 1) = 2.0f * (q23 + q01);
dcm(1, 0) = 2.0f * (q12 - q03);
dcm(2, 0) = 2.0f * (q13 + q02);
dcm(0, 1) = 2.0f * (q12 + q03);
dcm(2, 1) = 2.0f * (q23 - q01);
dcm(0, 2) = 2.0f * (q13 - q02);
dcm(1, 2) = 2.0f * (q23 + q01);
return dcm;
}

3
EKF/mag_fusion.cpp
View File

@ -73,8 +73,7 @@ void Ekf::fuseMag()
SH_MAG[8] = 2.0f*magE*q3;
// rotate magnetometer earth field state into body frame
Dcmf R_to_body(_state.quat_nominal);
R_to_body = R_to_body.transpose();
Dcmf R_to_body = quat_to_invrotmat(_state.quat_nominal);
Vector3f mag_I_rot = R_to_body * _state.mag_I;

3
EKF/optflow_fusion.cpp
View File

@ -68,8 +68,7 @@ void Ekf::fuseOptFlow()
float Kfusion[24][2] = {}; // Optical flow Kalman gains
// get rotation matrix from earth to body
Dcmf earth_to_body(_state.quat_nominal);
earth_to_body = earth_to_body.transpose();
Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
// calculate the sensor position relative to the IMU
Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;

3
EKF/sideslip_fusion.cpp
View File

@ -73,8 +73,7 @@ void Ekf::fuseSideslip()
rel_wind(1) = ve - vwe;
rel_wind(2) = vd;
Dcmf earth_to_body(_state.quat_nominal);
earth_to_body = earth_to_body.transpose(); //Why transpose?
Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
// rotate into body axes
rel_wind = earth_to_body * rel_wind;

3
EKF/terrain_estimator.cpp
View File

@ -204,8 +204,7 @@ void Ekf::fuseFlowForTerrain()
float R_LOS = calcOptFlowMeasVar();
// get rotation matrix from earth to body
Dcmf earth_to_body(_state.quat_nominal);
earth_to_body = earth_to_body.transpose();
Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);
// calculate the sensor position relative to the IMU
Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;