void BlockLocalPositionEstimator::gpsCorrect()
{
// measure
Vector<double, n_y_gps> y_global;
if (gpsMeasure(y_global) != OK) { return; }
// gps measurement in local frame
double lat = y_global(Y_gps_x);
double lon = y_global(Y_gps_y);
float alt = y_global(Y_gps_z);
float px = 0;
float py = 0;
float pz = -(alt - _gpsAltOrigin);
map_projection_project(&_map_ref, lat, lon, &px, &py);
Vector<float, n_y_gps> y;
y.setZero();
y(Y_gps_x) = px;
y(Y_gps_y) = py;
y(Y_gps_z) = pz;
y(Y_gps_vx) = y_global(Y_gps_vx);
y(Y_gps_vy) = y_global(Y_gps_vy);
y(Y_gps_vz) = y_global(Y_gps_vz);
// gps measurement matrix, measures position and velocity
Matrix<float, n_y_gps, n_x> C;
C.setZero();
C(Y_gps_x, X_x) = 1;
C(Y_gps_y, X_y) = 1;
C(Y_gps_z, X_z) = 1;
C(Y_gps_vx, X_vx) = 1;
C(Y_gps_vy, X_vy) = 1;
C(Y_gps_vz, X_vz) = 1;
// gps covariance matrix
SquareMatrix<float, n_y_gps> R;
R.setZero();
// default to parameter, use gps cov if provided
float var_xy = _param_lpe_gps_xy.get() * _param_lpe_gps_xy.get();
float var_z = _param_lpe_gps_z.get() * _param_lpe_gps_z.get();
float var_vxy = _param_lpe_gps_vxy.get() * _param_lpe_gps_vxy.get();
float var_vz = _param_lpe_gps_vz.get() * _param_lpe_gps_vz.get();
// if field is not below minimum, set it to the value provided
if (_sub_gps.get().eph > _param_lpe_gps_xy.get()) {
var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
}
if (_sub_gps.get().epv > _param_lpe_gps_z.get()) {
var_z = _sub_gps.get().epv * _sub_gps.get().epv;
}
float gps_s_stddev = _sub_gps.get().s_variance_m_s;
if (gps_s_stddev > _param_lpe_gps_vxy.get()) {
var_vxy = gps_s_stddev * gps_s_stddev;
}
if (gps_s_stddev > _param_lpe_gps_vz.get()) {
var_vz = gps_s_stddev * gps_s_stddev;
}
R(0, 0) = var_xy;
R(1, 1) = var_xy;
R(2, 2) = var_z;
R(3, 3) = var_vxy;
R(4, 4) = var_vxy;
R(5, 5) = var_vz;
// get delayed x
uint8_t i_hist = 0;
if (getDelayPeriods(_param_lpe_gps_delay.get(), &i_hist) < 0) { return; }
Vector<float, n_x> x0 = _xDelay.get(i_hist);
// residual
Vector<float, n_y_gps> r = y - C * x0;
// residual covariance
Matrix<float, n_y_gps, n_y_gps> S = C * _P * C.transpose() + R;
// publish innovations
for (size_t i = 0; i < 6; i++) {
_pub_innov.get().vel_pos_innov[i] = r(i);
_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
}
// residual covariance, (inverse)
Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, n_y_gps>(S);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
// artifically increase beta threshhold to prevent fault during landing
float beta_thresh = 1e2f;
if (beta / BETA_TABLE[n_y_gps] > beta_thresh) {
if (!(_sensorFault & SENSOR_GPS)) {
mavlink_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
double(r(0) * r(0) / S_I(0, 0)), double(r(1) * r(1) / S_I(1, 1)), double(r(2) * r(2) / S_I(2, 2)),
double(r(3) * r(3) / S_I(3, 3)), double(r(4) * r(4) / S_I(4, 4)), double(r(5) * r(5) / S_I(5, 5)));
_sensorFault |= SENSOR_GPS;
}
} else if (_sensorFault & SENSOR_GPS) {
_sensorFault &= ~SENSOR_GPS;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK");
}
// kalman filter correction always for GPS
Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
}
void BlockLocalPositionEstimator::visionCorrect()
{
// measure
Vector<float, n_y_vision> y;
if (visionMeasure(y) != OK) { return; }
// vision measurement matrix, measures position
Matrix<float, n_y_vision, n_x> C;
C.setZero();
C(Y_vision_x, X_x) = 1;
C(Y_vision_y, X_y) = 1;
C(Y_vision_z, X_z) = 1;
// noise matrix
Matrix<float, n_y_vision, n_y_vision> R;
R.setZero();
// use error estimates from vision topic if available
if (_sub_vision_pos.get().eph > _vision_xy_stddev.get()) {
R(Y_vision_x, Y_vision_x) = _sub_vision_pos.get().eph * _sub_vision_pos.get().eph;
R(Y_vision_y, Y_vision_y) = _sub_vision_pos.get().eph * _sub_vision_pos.get().eph;
} else {
R(Y_vision_x, Y_vision_x) = _vision_xy_stddev.get() * _vision_xy_stddev.get();
R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get();
}
if (_sub_vision_pos.get().epv > _vision_z_stddev.get()) {
R(Y_vision_z, Y_vision_z) = _sub_vision_pos.get().epv * _sub_vision_pos.get().epv;
} else {
R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get();
}
// vision delayed x
uint8_t i_hist = 0;
float vision_delay = (_timeStamp - _sub_vision_pos.get().timestamp) * 1e-6f; // measurement delay in seconds
if (vision_delay < 0.0f) { vision_delay = 0.0f; }
// use auto-calculated delay from measurement if parameter is set to zero
if (getDelayPeriods(_vision_delay.get() > 0.0f ? _vision_delay.get() : vision_delay, &i_hist) < 0) { return; }
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision delay : %0.2f ms", double(vision_delay * 1000.0f));
Vector<float, n_x> x0 = _xDelay.get(i_hist);
// residual
Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R);
Matrix<float, n_y_vision, 1> r = y - C * x0;
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_vision]) {
if (!(_sensorFault & SENSOR_VISION)) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_VISION;
}
} else if (_sensorFault & SENSOR_VISION) {
_sensorFault &= ~SENSOR_VISION;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position OK");
}
// kalman filter correction if no fault
if (!(_sensorFault & SENSOR_VISION)) {
Matrix<float, n_x, n_y_vision> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
}
}
