What theory of local position estimator is?

1

In the lpe,the function of covariance prediect as followed:

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);
	// 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());

}

But I read the book about continuous time kalman filter,It show the formula of it as followed:

2017-04-18_20-00-29.png585×597 30.8 KB

2

the prediect covariance formula of the code doesn’t match for the formula of the book.
why?

3

The formulas you posted are the analytical functions of the continuous time Kalman filter. The flight controller, being a computer, updates the Kalman filter in discrete time steps. Thus, the continuous time functions have to be approximated, which is done with the Runge Kutta method. I would recommend reading the related Wikipedia article referenced in the code snipped you posted.

4

Thanks for your reply.
The Wikipedia article is only about Runge Kutta method.
But the solve method of dP in the code is not illustrated.

5

Your book is showing the formula for the Kalman Filter, which is based on a linear system (8.36). The Local Position Estimator uses the Extended Kalman Filter which is a workaround for estimating a non-linear system.

See this paper on the derivation of the extended Kalman Filter. See Section B, Equation (8) for the covariance propagation step and Table 1 for the Runge Kutta values.