Browse Source

Support 3d auxvel fusion

sbg
kamilritz 5 years ago committed by Julian Kent
parent
commit
90aa4bb852
  1. 4
      src/modules/ekf2/ekf2_main.cpp

4
src/modules/ekf2/ekf2_main.cpp

@ -1177,8 +1177,8 @@ void Ekf2::Run() @@ -1177,8 +1177,8 @@ void Ekf2::Run()
// we can only use the landing target if it has a fixed position and a valid velocity estimate
if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) {
// velocity of vehicle relative to target has opposite sign to target relative to vehicle
float velocity[2] = { -landing_target_pose.vx_rel, -landing_target_pose.vy_rel};
float variance[2] = {landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel};
matrix::Vector3f velocity { -landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f};
matrix::Vector3f variance {landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, 0.0f};
_ekf.setAuxVelData(landing_target_pose.timestamp, velocity, variance);
}
}

Loading…
Cancel
Save