void BlockLocalPositionEstimator::updateSSStates()
{
// derivative of velocity is accelerometer acceleration
// (in input matrix) - bias (in body frame)
Matrix3f R_att(_sub_att.get().R);
_A(X_vx, X_bx) = -R_att(0, 0);
_A(X_vx, X_by) = -R_att(0, 1);
_A(X_vx, X_bz) = -R_att(0, 2);
_A(X_vy, X_bx) = -R_att(1, 0);
_A(X_vy, X_by) = -R_att(1, 1);
_A(X_vy, X_bz) = -R_att(1, 2);
_A(X_vz, X_bx) = -R_att(2, 0);
_A(X_vz, X_by) = -R_att(2, 1);
_A(X_vz, X_bz) = -R_att(2, 2);
}
void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitude,
const struct sensor_combined_s *sensor,
const struct distance_sensor_s *distance)
{
if (attitude->R_valid) {
matrix::Matrix<float, 3, 3> R_att(attitude->R);
matrix::Vector<float, 3> a(&sensor->accelerometer_m_s2[0]);
matrix::Vector<float, 3> u;
u = R_att * a;
_u_z = u(2) + 9.81f; // compensate for gravity
} else {
_u_z = 0.0f;
}
// dynamics matrix
matrix::Matrix<float, n_x, n_x> A;
A.setZero();
A(0, 1) = 1;
A(1, 2) = 1;
// input matrix
matrix::Matrix<float, n_x, 1> B;
B.setZero();
B(1, 0) = 1;
// input noise variance
float R = 0.135f;
// process noise convariance
matrix::Matrix<float, n_x, n_x> Q;
Q(0, 0) = 0;
Q(1, 1) = 0;
// do prediction
matrix::Vector<float, n_x> dx = (A * _x) * dt;
dx(1) += B(1, 0) * _u_z * dt;
// propagate state and covariance matrix
_x += dx;
_P += (A * _P + _P * A.transpose() +
B * R * B.transpose() + Q) * dt;
}
void BlockLocalPositionEstimator::predict()
{
// if can't update anything, don't propagate
// state or covariance
if (!_validXY && !_validZ) { return; }
if (integrate && _sub_att.get().R_valid) {
Matrix3f R_att(_sub_att.get().R);
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
_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;
_P += (_A * _P + _P * _A.transpose() +
_B * _R * _B.transpose() +
_Q) * getDt();
_xLowPass.update(_x);
}