fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 03:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Support odometry velocity in body and local frame (#14703)

Browse Source 
* Update submodule ECL

* increase lower bound on EVV param
This commit is contained in:
kritz
2020-05-13 12:43:02 +02:00
committed by GitHub
parent 8804dae480
commit 3897030c6f
8 changed files with 86 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+37 -9
View File
@@ -1117,18 +1117,28 @@ void Ekf2::Run()
ev_data.vel(1) = _ev_odom.vy;
ev_data.vel(2) = _ev_odom.vz;
if (_ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) {
ev_data.vel_frame = estimator::BODY_FRAME_FRD;
} else {
ev_data.vel_frame = estimator::LOCAL_FRAME_FRD;
}
// velocity measurement error from ev_data or parameters
float param_evv_noise_var = sq(_param_ekf2_evv_noise.get());
if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(_ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE])
&& PX4_ISFINITE(_ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VY_VARIANCE])
&& PX4_ISFINITE(_ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE])) {
ev_data.velVar(0) = fmaxf(param_evv_noise_var, _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE]);
ev_data.velVar(1) = fmaxf(param_evv_noise_var, _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VY_VARIANCE]);
ev_data.velVar(2) = fmaxf(param_evv_noise_var, _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE]);
ev_data.velCov(0, 0) = _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE];
ev_data.velCov(0, 1) = ev_data.velCov(1, 0) = _ev_odom.velocity_covariance[1];
ev_data.velCov(0, 2) = ev_data.velCov(2, 0) = _ev_odom.velocity_covariance[2];
ev_data.velCov(1, 1) = _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VY_VARIANCE];
ev_data.velCov(1, 2) = ev_data.velCov(2, 1) = _ev_odom.velocity_covariance[7];
ev_data.velCov(2, 2) = _ev_odom.velocity_covariance[_ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE];
} else {
ev_data.velVar.setAll(param_evv_noise_var);
ev_data.velCov = matrix::eye<float, 3>() * param_evv_noise_var;
}
}
@@ -1234,7 +1244,7 @@ void Ekf2::Run()
odom.timestamp = hrt_absolute_time();
odom.timestamp_sample = now;
odom.local_frame = odom.LOCAL_FRAME_NED;
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
// Position of body origin in local NED frame
Vector3f position = _ekf.getPosition();
@@ -1256,6 +1266,7 @@ void Ekf2::Run()
lpos.vz = velocity(2);
// Vehicle odometry linear velocity
odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
odom.vx = lpos.vx;
odom.vy = lpos.vy;
odom.vz = lpos.vz;
@@ -1417,10 +1428,27 @@ void Ekf2::Run()
aligned_ev_odom.y = aligned_pos(1);
aligned_ev_odom.z = aligned_pos(2);
const Vector3f aligned_vel = ev_rot_mat * Vector3f(_ev_odom.vx, _ev_odom.vy, _ev_odom.vz);
aligned_ev_odom.vx = aligned_vel(0);
aligned_ev_odom.vy = aligned_vel(1);
aligned_ev_odom.vz = aligned_vel(2);
switch (_ev_odom.velocity_frame) {
case vehicle_odometry_s::BODY_FRAME_FRD: {
const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) *
Vector3f(_ev_odom.vx, _ev_odom.vy, _ev_odom.vz);
aligned_ev_odom.vx = aligned_vel(0);
aligned_ev_odom.vy = aligned_vel(1);
aligned_ev_odom.vz = aligned_vel(2);
break;
}
case vehicle_odometry_s::LOCAL_FRAME_FRD: {
const Vector3f aligned_vel = ev_rot_mat *
Vector3f(_ev_odom.vx, _ev_odom.vy, _ev_odom.vz);
aligned_ev_odom.vx = aligned_vel(0);
aligned_ev_odom.vy = aligned_vel(1);
aligned_ev_odom.vz = aligned_vel(2);
break;
}
}
aligned_ev_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
// Compute orientation in EKF navigation frame
Quatf ev_quat_aligned = quat_ev2ekf * matrix::Quatf(_ev_odom.q) ;
src/modules/ekf2/ekf2_params.c
+1 -1
View File
@@ -750,7 +750,7 @@ PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f);
* Measurement noise for vision angle observations used to lower bound or replace the uncertainty included in the message
*
* @group EKF2
* @min 0.01
* @min 0.05
* @unit rad
* @decimal 2
*/