void BlockLocalPositionEstimator::mocapInit()
{
// measure
Vector<float, n_y_mocap> y;
if (mocapMeasure(y) != OK) {
_mocapStats.reset();
return;
}
// if finished
if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
_mocapHome = _mocapStats.getMean();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap position init: "
"%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
double(_mocapStats.getMean()(0)),
double(_mocapStats.getMean()(1)),
double(_mocapStats.getMean()(2)),
double(_mocapStats.getStdDev()(0)),
double(_mocapStats.getStdDev()(1)),
double(_mocapStats.getStdDev()(2)));
_mocapInitialized = true;
_mocapFault = FAULT_NONE;
if (!_altHomeInitialized) {
_altHomeInitialized = true;
_altHome = _mocapHome(2);
}
}
}
void BlockLocalPositionEstimator::mocapCorrect()
{
// measure
Vector<float, n_y_mocap> y;
if (mocapMeasure(y) != OK) { return; }
// make measurement relative to origin
y -= _mocapOrigin;
// mocap measurement matrix, measures position
Matrix<float, n_y_mocap, n_x> C;
C.setZero();
C(Y_mocap_x, X_x) = 1;
C(Y_mocap_y, X_y) = 1;
C(Y_mocap_z, X_z) = 1;
// noise matrix
Matrix<float, n_y_mocap, n_y_mocap> R;
R.setZero();
float mocap_p_var = _mocap_p_stddev.get()* \
_mocap_p_stddev.get();
R(Y_mocap_x, Y_mocap_x) = mocap_p_var;
R(Y_mocap_y, Y_mocap_y) = mocap_p_var;
R(Y_mocap_z, Y_mocap_z) = mocap_p_var;
// residual
Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R);
Matrix<float, n_y_mocap, 1> r = y - C * _x;
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_mocap]) {
if (_mocapFault < FAULT_MINOR) {
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta));
_mocapFault = FAULT_MINOR;
}
} else if (_mocapFault) {
_mocapFault = FAULT_NONE;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap OK");
}
// kalman filter correction if no fault
if (_mocapFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
correctionLogic(dx);
_x += dx;
_P -= K * C * _P;
}
}
void BlockLocalPositionEstimator::mocapInit()
{
// measure
Vector<float, n_y_mocap> y;
if (mocapMeasure(y) != OK) {
_mocapStats.reset();
return;
}
// if finished
if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap position init: "
"%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
double(_mocapStats.getMean()(0)),
double(_mocapStats.getMean()(1)),
double(_mocapStats.getMean()(2)),
double(_mocapStats.getStdDev()(0)),
double(_mocapStats.getStdDev()(1)),
double(_mocapStats.getStdDev()(2)));
_sensorTimeout &= ~SENSOR_MOCAP;
_sensorFault &= ~SENSOR_MOCAP;
// get reference for global position
globallocalconverter_getref(&_ref_lat, &_ref_lon, &_ref_alt);
_global_ref_timestamp = _timeStamp;
_is_global_cov_init = globallocalconverter_initialized();
if (!_map_ref.init_done && _is_global_cov_init && !_visionUpdated) {
// initialize global origin using the mocap estimator reference (only if the vision estimation is not being fused as well)
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (mocap) : lat %6.2f lon %6.2f alt %5.1f m",
double(_ref_lat), double(_ref_lon), double(_ref_alt));
map_projection_init(&_map_ref, _ref_lat, _ref_lon);
// set timestamp when origin was set to current time
_time_origin = _timeStamp;
}
if (!_altOriginInitialized) {
_altOriginInitialized = true;
_altOriginGlobal = true;
_altOrigin = globallocalconverter_initialized() ? _ref_alt : 0.0f;
}
}
}
void BlockLocalPositionEstimator::mocapCorrect()
{
// measure
Vector<float, n_y_mocap> y;
if (mocapMeasure(y) != OK) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", _mocap_eph, _mocap_epv);
return;
}
// mocap measurement matrix, measures position
Matrix<float, n_y_mocap, n_x> C;
C.setZero();
C(Y_mocap_x, X_x) = 1;
C(Y_mocap_y, X_y) = 1;
C(Y_mocap_z, X_z) = 1;
// noise matrix
Matrix<float, n_y_mocap, n_y_mocap> R;
R.setZero();
// use std dev from mocap data if available
if (_mocap_eph > _param_lpe_vic_p.get()) {
R(Y_mocap_x, Y_mocap_x) = _mocap_eph * _mocap_eph;
R(Y_mocap_y, Y_mocap_y) = _mocap_eph * _mocap_eph;
} else {
R(Y_mocap_x, Y_mocap_x) = _param_lpe_vic_p.get() * _param_lpe_vic_p.get();
R(Y_mocap_y, Y_mocap_y) = _param_lpe_vic_p.get() * _param_lpe_vic_p.get();
}
if (_mocap_epv > _param_lpe_vic_p.get()) {
R(Y_mocap_z, Y_mocap_z) = _mocap_epv * _mocap_epv;
} else {
R(Y_mocap_z, Y_mocap_z) = _param_lpe_vic_p.get() * _param_lpe_vic_p.get();
}
// residual
Vector<float, n_y_mocap> r = y - C * _x;
// residual covariance
Matrix<float, n_y_mocap, n_y_mocap> S = C * _P * C.transpose() + R;
// publish innovations
for (size_t i = 0; i < 3; i++) {
_pub_innov.get().vel_pos_innov[i] = r(i);
_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
}
for (size_t i = 3; i < 6; i++) {
_pub_innov.get().vel_pos_innov[i] = 0;
_pub_innov.get().vel_pos_innov_var[i] = 1;
}
// residual covariance, (inverse)
Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>(S);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_mocap]) {
if (!(_sensorFault & SENSOR_MOCAP)) {
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_MOCAP;
}
} else if (_sensorFault & SENSOR_MOCAP) {
_sensorFault &= ~SENSOR_MOCAP;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap OK");
}
// kalman filter correction always
Matrix<float, n_x, n_y_mocap> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
}
