void BlockLocalPositionEstimator::flowCorrect()
{
// measure flow
Vector<float, n_y_flow> y;
if (flowMeasure(y) != OK) { return; }
// flow measurement matrix and noise matrix
Matrix<float, n_y_flow, n_x> C;
C.setZero();
C(Y_flow_x, X_x) = 1;
C(Y_flow_y, X_y) = 1;
Matrix<float, n_y_flow, n_y_flow> R;
R.setZero();
R(Y_flow_x, Y_flow_x) =
_flow_xy_stddev.get() * _flow_xy_stddev.get();
R(Y_flow_y, Y_flow_y) =
_flow_xy_stddev.get() * _flow_xy_stddev.get();
// residual
Vector<float, 2> r = y - C * _x;
// residual covariance, (inverse)
Matrix<float, n_y_flow, n_y_flow> S_I =
inv<float, n_y_flow>(C * _P * C.transpose() + R);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_flow]) {
if (_flowFault < FAULT_MINOR) {
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta));
_flowFault = FAULT_MINOR;
}
} else if (_flowFault) {
_flowFault = FAULT_NONE;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK");
}
if (_flowFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_flow> K =
_P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
correctionLogic(dx);
_x += dx;
_P -= K * C * _P;
} else {
// reset flow integral to current estimate of position
// if a fault occurred
_flowX = _x(X_x);
_flowY = _x(X_y);
}
}
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::visionCorrect()
{
// measure
Vector<float, n_y_vision> y;
if (visionMeasure(y) != OK) { return; }
// make measurement relative to home
y -= _visionHome;
// 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::baroCorrect()
{
// measure
Vector<float, n_y_baro> y;
if (baroMeasure(y) != OK) { return; }
// subtract baro home alt
y -= _baroAltHome;
// baro measurement matrix
Matrix<float, n_y_baro, n_x> C;
C.setZero();
C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir.
Matrix<float, n_y_baro, n_y_baro> R;
R.setZero();
R(0, 0) = _baro_stddev.get() * _baro_stddev.get();
// residual
Matrix<float, n_y_baro, n_y_baro> S_I =
inv<float, n_y_baro>((C * _P * C.transpose()) + R);
Vector<float, n_y_baro> r = y - (C * _x);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_baro]) {
if (_baroFault < FAULT_MINOR) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
_baroFault = FAULT_MINOR;
}
} else if (_baroFault) {
_baroFault = FAULT_NONE;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro OK");
}
// kalman filter correction if no fault
if (_baroFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_baro> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
correctionLogic(dx);
_x += dx;
_P -= K * C * _P;
}
}
void BlockLocalPositionEstimator::predict()
{
// if can't update anything, don't propagate
// state or covariance
if (!_validXY && !_validZ) { return; }
if (integrate) {
matrix::Quaternion<float> q(&_sub_att.get().q[0]);
_eul = matrix::Euler<float>(q);
_R_att = matrix::Dcm<float>(q);
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
// note, bias is removed in dynamics function
_u = _R_att * a;
_u(U_az) += 9.81f; // add g
} else {
_u = Vector3f(0, 0, 0);
}
// update state space based on new states
updateSSStates();
// continuous time kalman filter prediction
// integrate runge kutta 4th order
// TODO move rk4 algorithm to matrixlib
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
float h = getDt();
Vector<float, n_x> k1, k2, k3, k4;
k1 = dynamics(0, _x, _u);
k2 = dynamics(h / 2, _x + k1 * h / 2, _u);
k3 = dynamics(h / 2, _x + k2 * h / 2, _u);
k4 = dynamics(h, _x + k3 * h, _u);
Vector<float, n_x> dx = (k1 + k2 * 2 + k3 * 2 + k4) * (h / 6);
// propagate
correctionLogic(dx);
_x += dx;
Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
_B * _R * _B.transpose() + _Q) * getDt();
covPropagationLogic(dP);
_P += dP;
_xLowPass.update(_x);
_aglLowPass.update(agl());
}
