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);
}
Exemplo n.º 2
0
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);
}