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());
}
void BlockLocalPositionEstimator::initSS()
{
initP();
// dynamics matrix
_A.setZero();
// derivative of position is velocity
_A(X_x, X_vx) = 1;
_A(X_y, X_vy) = 1;
_A(X_z, X_vz) = 1;
// input matrix
_B.setZero();
_B(X_vx, U_ax) = 1;
_B(X_vy, U_ay) = 1;
_B(X_vz, U_az) = 1;
// update components that depend on current state
updateSSStates();
updateSSParams();
}
void BlockLocalPositionEstimator::predict()
{
// get acceleration
matrix::Quatf 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
// 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);
// don't integrate position if no valid xy data
if (!(_estimatorInitialized & EST_XY)) {
dx(X_x) = 0;
dx(X_vx) = 0;
dx(X_y) = 0;
dx(X_vy) = 0;
}
// don't integrate z if no valid z data
if (!(_estimatorInitialized & EST_Z)) {
dx(X_z) = 0;
}
// don't integrate tz if no valid tz data
if (!(_estimatorInitialized & EST_TZ)) {
dx(X_tz) = 0;
}
// saturate bias
float bx = dx(X_bx) + _x(X_bx);
float by = dx(X_by) + _x(X_by);
float bz = dx(X_bz) + _x(X_bz);
if (std::abs(bx) > BIAS_MAX) {
bx = BIAS_MAX * bx / std::abs(bx);
dx(X_bx) = bx - _x(X_bx);
}
if (std::abs(by) > BIAS_MAX) {
by = BIAS_MAX * by / std::abs(by);
dx(X_by) = by - _x(X_by);
}
if (std::abs(bz) > BIAS_MAX) {
bz = BIAS_MAX * bz / std::abs(bz);
dx(X_bz) = bz - _x(X_bz);
}
// propagate
_x += dx;
Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
_B * _R * _B.transpose() + _Q) * getDt();
// covariance propagation logic
for (int i = 0; i < n_x; i++) {
if (_P(i, i) > P_MAX) {
// if diagonal element greater than max, stop propagating
dP(i, i) = 0;
for (int j = 0; j < n_x; j++) {
dP(i, j) = 0;
dP(j, i) = 0;
}
}
}
_P += dP;
_xLowPass.update(_x);
_aglLowPass.update(agl());
}
