What theory of local position estimator is?


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

// 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
_x += dx;
Matrix<float, n_x, n_x> dP = (_A * _P + _P * _A.transpose() +
			      _B * _R * _B.transpose() + _Q) * getDt();
_P += dP;



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


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


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.


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.


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.