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; } }