mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-19 21:03:35 +08:00
EKF: fixed some comment typos
Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
This commit is contained in:
Carl Olsson
+14
-15
@@ -168,7 +168,7 @@ bool Ekf::resetPosition()
|
||||
|
||||
}
|
||||
|
||||
// estimate is relative to initial positon in this mode, so we start with zero error.
|
||||
// estimate is relative to initial position in this mode, so we start with zero error.
|
||||
zeroCols(P,7,8);
|
||||
zeroRows(P,7,8);
|
||||
|
||||
@@ -223,7 +223,7 @@ void Ekf::resetHeight()
|
||||
// calculate the new vertical position using range sensor
|
||||
float new_pos_down = _hgt_sensor_offset - range_newest.rng * _R_rng_to_earth_2_2;
|
||||
|
||||
// update the state and assoicated variance
|
||||
// update the state and associated variance
|
||||
_state.pos(2) = new_pos_down;
|
||||
|
||||
// reset the associated covariance values
|
||||
@@ -269,7 +269,7 @@ void Ekf::resetHeight()
|
||||
if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) {
|
||||
_state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref;
|
||||
|
||||
// reset the associated covarince values
|
||||
// reset the associated covariance values
|
||||
zeroRows(P, 9, 9);
|
||||
zeroCols(P, 9, 9);
|
||||
|
||||
@@ -382,12 +382,11 @@ void Ekf::alignOutputFilter()
|
||||
Quatf q_delta = _output_sample_delayed.quat_nominal.inversed() * _state.quat_nominal;
|
||||
q_delta.normalize();
|
||||
|
||||
// calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon
|
||||
// calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon
|
||||
const Vector3f vel_delta = _state.vel - _output_sample_delayed.vel;
|
||||
const Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
|
||||
|
||||
// loop through the output filter state history and add the deltas
|
||||
// Note q1 *= q2 is equivalent to q1 = q1 * q2
|
||||
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
|
||||
_output_buffer[i].quat_nominal *= q_delta;
|
||||
_output_buffer[i].quat_nominal.normalize();
|
||||
@@ -434,7 +433,7 @@ bool Ekf::realignYawGPS()
|
||||
if (badMagYaw || !_control_status.flags.yaw_align) {
|
||||
ECL_WARN("EKF bad yaw corrected using GPS course");
|
||||
|
||||
// declare the magnetomer as failed if a bad yaw has occurred more than once
|
||||
// declare the magnetometer as failed if a bad yaw has occurred more than once
|
||||
if (_control_status.flags.mag_align_complete && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) {
|
||||
ECL_WARN("EKF stopping magnetometer use");
|
||||
_control_status.flags.mag_fault = true;
|
||||
@@ -451,7 +450,7 @@ bool Ekf::realignYawGPS()
|
||||
|
||||
// apply yaw correction
|
||||
if (!_control_status.flags.mag_align_complete) {
|
||||
// This is our first flight aligment so we can assume that the recent change in velocity has occurred due to a
|
||||
// This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a
|
||||
// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
|
||||
euler321(2) += courseYawError;
|
||||
_control_status.flags.mag_align_complete = true;
|
||||
@@ -468,7 +467,7 @@ bool Ekf::realignYawGPS()
|
||||
|
||||
}
|
||||
|
||||
// calculate new filter quaternion states using corected yaw angle
|
||||
// calculate new filter quaternion states using corrected yaw angle
|
||||
_state.quat_nominal = Quatf(euler321);
|
||||
uncorrelateQuatStates();
|
||||
|
||||
@@ -566,7 +565,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update
|
||||
}
|
||||
|
||||
if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) {
|
||||
// do not use the magnetomer and deactivate magnetic field states
|
||||
// do not use the magnetometer and deactivate magnetic field states
|
||||
// save covariance data for re-use if currently doing 3-axis fusion
|
||||
if (_control_status.flags.mag_3D) {
|
||||
save_mag_cov_data();
|
||||
@@ -851,7 +850,7 @@ void Ekf::get_vel_pos_innov(float vel_pos_innov[6])
|
||||
memcpy(vel_pos_innov, _vel_pos_innov, sizeof(float) * 6);
|
||||
}
|
||||
|
||||
// gets the innovations of the earth magnetic field measurements
|
||||
// gets the innovations for of the NE auxiliary velocity measurement
|
||||
void Ekf::get_aux_vel_innov(float aux_vel_innov[2])
|
||||
{
|
||||
memcpy(aux_vel_innov, _aux_vel_innov, sizeof(float) * 2);
|
||||
@@ -863,7 +862,7 @@ void Ekf::get_mag_innov(float mag_innov[3])
|
||||
memcpy(mag_innov, _mag_innov, 3 * sizeof(float));
|
||||
}
|
||||
|
||||
// gets the innovations of the airspeed measnurement
|
||||
// gets the innovations of the airspeed measurement
|
||||
void Ekf::get_airspeed_innov(float *airspeed_innov)
|
||||
{
|
||||
memcpy(airspeed_innov, &_airspeed_innov, sizeof(float));
|
||||
@@ -894,7 +893,7 @@ void Ekf::get_mag_innov_var(float mag_innov_var[3])
|
||||
memcpy(mag_innov_var, _mag_innov_var, sizeof(float) * 3);
|
||||
}
|
||||
|
||||
// gest the innovation variance of the airspeed measurement
|
||||
// gets the innovation variance of the airspeed measurement
|
||||
void Ekf::get_airspeed_innov_var(float *airspeed_innov_var)
|
||||
{
|
||||
memcpy(airspeed_innov_var, &_airspeed_innov_var, sizeof(float));
|
||||
@@ -1168,10 +1167,10 @@ bool Ekf::reset_imu_bias()
|
||||
// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
|
||||
// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
|
||||
// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
|
||||
// Where a measurement type is a vector quantity, eg magnetoemter, GPS position, etc, the maximum value is returned.
|
||||
// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
|
||||
void Ekf::get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl, float *beta)
|
||||
{
|
||||
// return the integer bitmask containing the consistency check pass/fail satus
|
||||
// return the integer bitmask containing the consistency check pass/fail status
|
||||
*status = _innov_check_fail_status.value;
|
||||
// return the largest magnetometer innovation test ratio
|
||||
*mag = sqrtf(math::max(_yaw_test_ratio, math::max(math::max(_mag_test_ratio[0], _mag_test_ratio[1]), _mag_test_ratio[2])));
|
||||
@@ -1593,7 +1592,7 @@ void Ekf::setControlEVHeight()
|
||||
}
|
||||
|
||||
// update the estimated misalignment between the EV navigation frame and the EKF navigation frame
|
||||
// and calculate a rotation matrix which rotates EV measurements into the EKF's navigatin frame
|
||||
// and calculate a rotation matrix which rotates EV measurements into the EKF's navigation frame
|
||||
void Ekf::calcExtVisRotMat()
|
||||
{
|
||||
// calculate the quaternion delta between the EKF and EV reference frames at the EKF fusion time horizon
|
||||
|
||||
Reference in New Issue
Block a user