void BlockLocalPositionEstimator::visionInit() { // measure Vector<float, n_y_vision> y; if (visionMeasure(y) != OK) { _visionStats.reset(); return; } // increament sums for mean if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) { _visionOrigin = _visionStats.getMean(); mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position init: " "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m", double(_visionStats.getMean()(0)), double(_visionStats.getMean()(1)), double(_visionStats.getMean()(2)), double(_visionStats.getStdDev()(0)), double(_visionStats.getStdDev()(1)), double(_visionStats.getStdDev()(2))); _visionInitialized = true; _visionFault = FAULT_NONE; if (!_altOriginInitialized) { _altOriginInitialized = true; _altOrigin = _visionOrigin(2); } } }
void BlockLocalPositionEstimator::visionCorrect() { // measure Vector<float, n_y_vision> y; if (visionMeasure(y) != OK) { return; } // make measurement relative to origin y -= _visionOrigin; // 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(); 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(); R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get(); // 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 * _x; // fault detection float beta = (r.transpose() * (S_I * r))(0, 0); if (beta > BETA_TABLE[n_y_vision]) { if (_visionFault < FAULT_MINOR) { //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta)); _visionFault = FAULT_MINOR; } } else if (_visionFault) { _visionFault = FAULT_NONE; //mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position OK"); } // kalman filter correction if no fault if (_visionFault < fault_lvl_disable) { Matrix<float, n_x, n_y_vision> K = _P * C.transpose() * S_I; Vector<float, n_x> dx = K * r; correctionLogic(dx); _x += dx; _P -= K * C * _P; } }
void BlockLocalPositionEstimator::visionInit() { // measure Vector<float, n_y_vision> y; if (visionMeasure(y) != OK) { _visionStats.reset(); return; } // increament sums for mean if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) { mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position init: " "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m", double(_visionStats.getMean()(0)), double(_visionStats.getMean()(1)), double(_visionStats.getMean()(2)), double(_visionStats.getStdDev()(0)), double(_visionStats.getStdDev()(1)), double(_visionStats.getStdDev()(2))); _sensorTimeout &= ~SENSOR_VISION; _sensorFault &= ~SENSOR_VISION; if (!_map_ref.init_done && _sub_vision_pos.get().xy_global) { // initialize global origin using the visual estimator reference mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (vision) : lat %6.2f lon %6.2f alt %5.1f m", double(_sub_vision_pos.get().ref_lat), double(_sub_vision_pos.get().ref_lon), double(_sub_vision_pos.get().ref_alt)); map_projection_init(&_map_ref, _sub_vision_pos.get().ref_lat, _sub_vision_pos.get().ref_lon); // set timestamp when origin was set to current time _time_origin = _timeStamp; } if (!_altOriginInitialized) { _altOriginInitialized = true; _altOrigin = _sub_vision_pos.get().z_global ? _sub_vision_pos.get().ref_alt : 0.0f; } } }
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; } }