If the GPS is mounted at an offset from the CoG, any angular velocity from the vehicle will induce a velocity due to the rotation, which is unwanted and needs to be corrected for.
In the GPS measurement model, this is accounted for, by subtracting the induced velocity caused by the positional offset. However, the uncorrected raw angular velocity is used.
}
}
}
}
void Ekf::updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src)
{
// correct velocity for offset relative to IMU
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
const Vector3f velocity = gnss_sample.vel - vel_offset_earth;
const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise));
const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));
updateVelocityAidSrcStatus(gnss_sample.time_us,
velocity, // observation
vel_obs_var, // observation variance
math::max(_params.gps_vel_innov_gate, 1.f), // innovation gate
aid_src);
_state.pos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f;
// constrain states
_state.vel = matrix::constrain(_state.vel, -1000.f, 1000.f);
_state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f);
// some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication
// protect against possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate
// due to insufficient averaging
if (imu_delayed.delta_ang_dt > 0.25f * _dt_ekf_avg) {
_ang_rate_delayed_raw = imu_delayed.delta_ang / imu_delayed.delta_ang_dt;
}
// calculate a filtered horizontal acceleration with a 1 sec time constant
// this are used for manoeuvre detection elsewhere
const float alpha = 1.0f - imu_delayed.delta_vel_dt;
_accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy();
// Calculate low pass filtered height rate
float alpha_height_rate_lpf = 0.1f * imu_delayed.delta_vel_dt; // 10 seconds time constant
What is the reason to use the uncorrected angular velocity?
