LPE Variance Dependent Publication (#4914)

* Use variance to control publishing for LPE. * Don't stop publishing if we have gps/ baro. * LPE tuning and cleanup. * Added bias saturation to LPE. * Added vector enabled low pass filter block. * Added rk4 integration and pub lowpass to LPE. * Fix std::abs issue on mac/ reset lowpass on state reset. * Don't estimate gyro bias when rotating at high speed att_est_q. * Lowered low pass on position to 5 Hz for LPE. * Streamline state space update for LPE. * Added health flags to est2 log. * Revert to old tuning, more conservative, less faults. * Formatting. * Fix for fault message on LPE. * Added subscription throttling to LPE. * Formatting.
2026-05-17 03:37:35 +08:00 · 2016-07-01 11:43:09 -04:00
parent 2a395c3fec
commit 00dfc99e08
17 changed files with 417 additions and 247 deletions
@@ -3,40 +3,45 @@
 #include <fcntl.h>
 #include <systemlib/err.h>
 #include <matrix/math.hpp>
+#include <cstdlib>

 orb_advert_t mavlink_log_pub = nullptr;

 // timeouts for sensors in microseconds
 static const uint32_t 		EST_SRC_TIMEOUT = 10000; // 0.01 s

+// required standard deviation of estimate for estimator to publish data
+static const uint32_t 		EST_STDDEV_XY_VALID = 2.0; // 2.0 m
+static const uint32_t 		EST_STDDEV_Z_VALID = 2.0; // 2.0 m
+static const uint32_t 		EST_STDDEV_TZ_VALID = 2.0; // 2.0 m
+
 // minimum flow altitude
 static const float flow_min_agl = 0.3;

 BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// this block has no parent, and has name LPE
 	SuperBlock(NULL, "LPE"),
-
 	// subscriptions, set rate, add to list
-	// TODO topic speed limiting?
-	_sub_status(ORB_ID(vehicle_status), 0, 0, &getSubscriptions()),
-	_sub_armed(ORB_ID(actuator_armed), 0, 0, &getSubscriptions()),
-	_sub_control_mode(ORB_ID(vehicle_control_mode),
-			  0, 0, &getSubscriptions()),
-	_sub_att(ORB_ID(vehicle_attitude), 0, 0, &getSubscriptions()),
-	_sub_att_sp(ORB_ID(vehicle_attitude_setpoint),
-		    0, 0, &getSubscriptions()),
-	_sub_flow(ORB_ID(optical_flow), 0, 0, &getSubscriptions()),
-	_sub_sensor(ORB_ID(sensor_combined), 0, 0, &getSubscriptions()),
-	_sub_param_update(ORB_ID(parameter_update), 0, 0, &getSubscriptions()),
-	_sub_manual(ORB_ID(manual_control_setpoint), 0, 0, &getSubscriptions()),
-	_sub_home(ORB_ID(home_position), 0, 0, &getSubscriptions()),
-	_sub_gps(ORB_ID(vehicle_gps_position), 0, 0, &getSubscriptions()),
-	_sub_vision_pos(ORB_ID(vision_position_estimate), 0, 0, &getSubscriptions()),
-	_sub_mocap(ORB_ID(att_pos_mocap), 0, 0, &getSubscriptions()),
-	_sub_dist0(ORB_ID(distance_sensor), 0, 0, &getSubscriptions()),
-	_sub_dist1(ORB_ID(distance_sensor), 0, 1, &getSubscriptions()),
-	_sub_dist2(ORB_ID(distance_sensor), 0, 2, &getSubscriptions()),
-	_sub_dist3(ORB_ID(distance_sensor), 0, 3, &getSubscriptions()),
+	_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
+	_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
+	// flow 10 hz
+	_sub_flow(ORB_ID(optical_flow), 1000 / 10, 0, &getSubscriptions()),
+	// main prediction loop, 100 hz
+	_sub_sensor(ORB_ID(sensor_combined), 1000 / 100, 0, &getSubscriptions()),
+	// status updates 2 hz
+	_sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()),
+	_sub_manual(ORB_ID(manual_control_setpoint), 1000 / 2, 0, &getSubscriptions()),
+	_sub_home(ORB_ID(home_position), 1000 / 2, 0, &getSubscriptions()),
+	// gps 10 hz
+	_sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),
+	// vision 5 hz
+	_sub_vision_pos(ORB_ID(vision_position_estimate), 1000 / 5, 0, &getSubscriptions()),
+	// all distance sensors, 10 hz
+	_sub_mocap(ORB_ID(att_pos_mocap), 1000 / 10, 0, &getSubscriptions()),
+	_sub_dist0(ORB_ID(distance_sensor), 1000 / 10, 0, &getSubscriptions()),
+	_sub_dist1(ORB_ID(distance_sensor), 1000 / 10, 1, &getSubscriptions()),
+	_sub_dist2(ORB_ID(distance_sensor), 1000 / 10, 2, &getSubscriptions()),
+	_sub_dist3(ORB_ID(distance_sensor), 1000 / 10, 3, &getSubscriptions()),
 	_dist_subs(),
 	_sub_lidar(NULL),
 	_sub_sonar(NULL),
@@ -97,7 +102,10 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_mocapStats(this, ""),
 	_gpsStats(this, ""),

-	// stats
+	// low pass
+	_xLowPass(this, "X_LP"),
+
+	// delay
 	_xDelay(this, ""),
 	_tDelay(this, ""),

@@ -141,9 +149,9 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_flowMeanQual(0),

 	// status
-	_canEstimateXY(false),
-	_canEstimateZ(false),
-	_canEstimateT(false),
+	_validXY(false),
+	_validZ(false),
+	_validTZ(false),
 	_xyTimeout(true),
 	_zTimeout(true),
 	_tzTimeout(true),
@@ -182,12 +190,12 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_polls[POLL_SENSORS].fd = _sub_sensor.getHandle();
 	_polls[POLL_SENSORS].events = POLLIN;

-	// initialize P, x, u
-	initP();
+	// initialize A, B,  P, x, u
 	_x.setZero();
 	_u.setZero();
 	_flowX = 0;
 	_flowY = 0;
+	initSS();

 	// perf counters
 	_loop_perf = perf_alloc(PC_ELAPSED,
@@ -207,6 +215,14 @@ BlockLocalPositionEstimator::~BlockLocalPositionEstimator()
 {
 }

+Vector<float, BlockLocalPositionEstimator::n_x> BlockLocalPositionEstimator::dynamics(
+	float t,
+	const Vector<float, BlockLocalPositionEstimator::n_x> &x,
+	const Vector<float, BlockLocalPositionEstimator::n_u> &u)
+{
+	return _A * x + _B * u;
+}
+
 void BlockLocalPositionEstimator::update()
 {

@@ -254,6 +270,9 @@ void BlockLocalPositionEstimator::update()

 		// assume we are on the ground, so terrain alt is local alt
 		_x(X_tz) = _x(X_z);
+
+		// reset lowpass filter as well
+		_xLowPass.setState(_x);
 	}

 	_lastArmedState = armedState;
@@ -275,6 +294,7 @@ void BlockLocalPositionEstimator::update()
 	// update parameters
 	if (paramsUpdated) {
 		updateParams();
+		updateSSParams();
 	}

 	// update home position projection
@@ -282,27 +302,60 @@ void BlockLocalPositionEstimator::update()
 		updateHome();
 	}

-	// determine if we should start estimating
-	_canEstimateZ =
-		(_baroInitialized && _baroFault < fault_lvl_disable);
-	_canEstimateXY =
-		(_gpsInitialized && _gpsFault < fault_lvl_disable) ||
-		(_flowInitialized && _flowFault < fault_lvl_disable) ||
-		(_visionInitialized && _visionFault < fault_lvl_disable) ||
-		(_mocapInitialized && _mocapFault < fault_lvl_disable);
-	_canEstimateT =
-		(_lidarInitialized && _lidarFault < fault_lvl_disable) ||
-		(_sonarInitialized && _sonarFault < fault_lvl_disable);
+	// is xy valid?
+	bool xy_stddev_ok = sqrt(math::max(_P(X_x, X_x), _P(X_y, X_y))) < EST_STDDEV_XY_VALID;

-	if (_canEstimateXY) {
+	if (_validXY) {
+		// if valid and gps has timed out, set to not valid
+		if (!xy_stddev_ok && !_gpsInitialized) {
+			_validXY = false;
+		}
+
+	} else {
+		if (xy_stddev_ok) {
+			_validXY = true;
+		}
+	}
+
+	// is z valid?
+	bool z_stddev_ok = sqrt(_P(X_z, X_z)) < EST_STDDEV_Z_VALID;
+
+	if (_validZ) {
+		// if valid and baro has timed out, set to not valid
+		if (!z_stddev_ok && !_baroInitialized) {
+			_validZ = false;
+		}
+
+	} else {
+		if (z_stddev_ok) {
+			_validZ = true;
+		}
+	}
+
+	// is terrain valid?
+	bool tz_stddev_ok = sqrt(_P(X_tz, X_tz)) < EST_STDDEV_TZ_VALID;
+
+	if (_validTZ) {
+		if (!tz_stddev_ok) {
+			_validTZ = false;
+		}
+
+	} else {
+		if (tz_stddev_ok) {
+			_validTZ = true;
+		}
+	}
+
+	// timeouts
+	if (_validXY) {
 		_time_last_xy = _timeStamp;
 	}

-	if (_canEstimateZ) {
+	if (_validZ) {
 		_time_last_z = _timeStamp;
 	}

-	if (_canEstimateT) {
+	if (_validTZ) {
 		_time_last_tz = _timeStamp;
 	}

@@ -310,7 +363,7 @@ void BlockLocalPositionEstimator::update()
 	checkTimeouts();

 	// if we have no lat, lon initialize projection at 0,0
-	if (_canEstimateXY && !_map_ref.init_done) {
+	if (_validXY && !_map_ref.init_done) {
 		map_projection_init(&_map_ref,
 				    _init_home_lat.get(),
 				    _init_home_lon.get());
@@ -431,7 +484,7 @@ void BlockLocalPositionEstimator::update()
 		publishLocalPos();
 		publishEstimatorStatus();

-		if (_canEstimateXY) {
+		if (_validXY) {
 			publishGlobalPos();
 		}
 	}
@@ -495,7 +548,53 @@ void BlockLocalPositionEstimator::checkTimeouts()

 float BlockLocalPositionEstimator::agl()
 {
-	return _x(X_tz) - _x(X_z);
+	const Vector<float, n_x> &xLP = _xLowPass.getState();
+	return xLP(X_tz) - xLP(X_z);
+}
+
+void BlockLocalPositionEstimator::correctionLogic(Vector<float, n_x> &dx)
+{
+	// don't correct bias when rotating rapidly
+	float ang_speed = sqrt(
+				  _sub_att.get().rollspeed * _sub_att.get().rollspeed +
+				  _sub_att.get().pitchspeed * _sub_att.get().pitchspeed +
+				  _sub_att.get().yawspeed * _sub_att.get().yawspeed);
+
+	if (ang_speed > 1) {
+		dx(X_bx) = 0;
+		dx(X_by) = 0;
+		dx(X_bz) = 0;
+	}
+
+	if (!_validXY) {
+		dx(X_x) = 0;
+		dx(X_y) = 0;
+		dx(X_vx) = 0;
+		dx(X_vy) = 0;
+		dx(X_bx) = 0;
+		dx(X_by) = 0;
+	}
+
+	if (!_validZ) {
+		dx(X_z) = 0;
+		dx(X_vz) = 0;
+		dx(X_bz) = 0;
+	}
+
+	if (!_validTZ) {
+		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); }
+
+	if (std::abs(by) > BIAS_MAX) { by = BIAS_MAX * by / std::abs(by); }
+
+	if (std::abs(bz) > BIAS_MAX) { bz = BIAS_MAX * bz / std::abs(bz); }
 }

 void BlockLocalPositionEstimator::detectDistanceSensors()
@@ -551,21 +650,23 @@ void BlockLocalPositionEstimator::updateHome()

 void BlockLocalPositionEstimator::publishLocalPos()
 {
+	const Vector<float, n_x> &xLP = _xLowPass.getState();
+
 	// publish local position
 	if (PX4_ISFINITE(_x(X_x)) && PX4_ISFINITE(_x(X_y)) && PX4_ISFINITE(_x(X_z)) &&
 	    PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy))
 	    && PX4_ISFINITE(_x(X_vz))) {
 		_pub_lpos.get().timestamp = _timeStamp;
-		_pub_lpos.get().xy_valid = _canEstimateXY;
-		_pub_lpos.get().z_valid = _canEstimateZ;
-		_pub_lpos.get().v_xy_valid = _canEstimateXY;
-		_pub_lpos.get().v_z_valid = _canEstimateZ;
-		_pub_lpos.get().x = _x(X_x); 	// north
-		_pub_lpos.get().y = _x(X_y);  	// east
-		_pub_lpos.get().z = _x(X_z); 	// down
-		_pub_lpos.get().vx = _x(X_vx);  // north
-		_pub_lpos.get().vy = _x(X_vy);  // east
-		_pub_lpos.get().vz = _x(X_vz); 	// down
+		_pub_lpos.get().xy_valid = _validXY;
+		_pub_lpos.get().z_valid = _validZ;
+		_pub_lpos.get().v_xy_valid = _validXY;
+		_pub_lpos.get().v_z_valid = _validZ;
+		_pub_lpos.get().x = xLP(X_x); 	// north
+		_pub_lpos.get().y = xLP(X_y);  	// east
+		_pub_lpos.get().z = xLP(X_z); 	// down
+		_pub_lpos.get().vx = xLP(X_vx); // north
+		_pub_lpos.get().vy = xLP(X_vy); // east
+		_pub_lpos.get().vz = xLP(X_vz); // down
 		_pub_lpos.get().yaw = _sub_att.get().yaw;
 		_pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference
 		_pub_lpos.get().z_global = _baroInitialized;
@@ -574,9 +675,9 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
 		_pub_lpos.get().ref_alt = _sub_home.get().alt;
 		_pub_lpos.get().dist_bottom = agl();
-		_pub_lpos.get().dist_bottom_rate = -_x(X_vz);
+		_pub_lpos.get().dist_bottom_rate = - xLP(X_vz);
 		_pub_lpos.get().surface_bottom_timestamp = _timeStamp;
-		_pub_lpos.get().dist_bottom_valid = _canEstimateZ;
+		_pub_lpos.get().dist_bottom_valid = _validTZ && _validZ;
 		_pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
 		_pub_lpos.get().epv = sqrtf(_P(X_z, X_z));
 		_pub_lpos.update();
@@ -595,13 +696,13 @@ void BlockLocalPositionEstimator::publishEstimatorStatus()
 	_pub_est_status.get().n_states = n_x;
 	_pub_est_status.get().nan_flags = 0;
 	_pub_est_status.get().health_flags =
-		((_baroFault > fault_lvl_disable) << SENSOR_BARO)
-		+ ((_gpsFault > fault_lvl_disable) << SENSOR_GPS)
-		+ ((_lidarFault > fault_lvl_disable) << SENSOR_LIDAR)
-		+ ((_flowFault > fault_lvl_disable) << SENSOR_FLOW)
-		+ ((_sonarFault > fault_lvl_disable) << SENSOR_SONAR)
-		+ ((_visionFault > fault_lvl_disable) << SENSOR_VISION)
-		+ ((_mocapFault > fault_lvl_disable) << SENSOR_MOCAP);
+		((_baroFault > FAULT_NONE) << SENSOR_BARO)
+		+ ((_gpsFault > FAULT_NONE) << SENSOR_GPS)
+		+ ((_lidarFault > FAULT_NONE) << SENSOR_LIDAR)
+		+ ((_flowFault > FAULT_NONE) << SENSOR_FLOW)
+		+ ((_sonarFault > FAULT_NONE) << SENSOR_SONAR)
+		+ ((_visionFault > FAULT_NONE) << SENSOR_VISION)
+		+ ((_mocapFault > FAULT_NONE) << SENSOR_MOCAP);
 	_pub_est_status.get().timeout_flags =
 		(_baroInitialized << SENSOR_BARO)
 		+ (_gpsInitialized << SENSOR_GPS)
@@ -618,26 +719,27 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 	// publish global position
 	double lat = 0;
 	double lon = 0;
-	map_projection_reproject(&_map_ref, _x(X_x), _x(X_y), &lat, &lon);
-	float alt = -_x(X_z) + _altHome;
+	const Vector<float, n_x> &xLP = _xLowPass.getState();
+	map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon);
+	float alt = -xLP(X_z) + _altHome;

 	if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt) &&
-	    PX4_ISFINITE(_x(X_vx)) && PX4_ISFINITE(_x(X_vy)) &&
-	    PX4_ISFINITE(_x(X_vz))) {
+	    PX4_ISFINITE(xLP(X_vx)) && PX4_ISFINITE(xLP(X_vy)) &&
+	    PX4_ISFINITE(xLP(X_vz))) {
 		_pub_gpos.get().timestamp = _timeStamp;
 		_pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec;
 		_pub_gpos.get().lat = lat;
 		_pub_gpos.get().lon = lon;
 		_pub_gpos.get().alt = alt;
-		_pub_gpos.get().vel_n = _x(X_vx);
-		_pub_gpos.get().vel_e = _x(X_vy);
-		_pub_gpos.get().vel_d = _x(X_vz);
+		_pub_gpos.get().vel_n = xLP(X_vx);
+		_pub_gpos.get().vel_e = xLP(X_vy);
+		_pub_gpos.get().vel_d = xLP(X_vz);
 		_pub_gpos.get().yaw = _sub_att.get().yaw;
 		_pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y));
 		_pub_gpos.get().epv = sqrtf(_P(X_z, X_z));
-		_pub_gpos.get().terrain_alt = _altHome - _x(X_tz);
-		_pub_gpos.get().terrain_alt_valid = _canEstimateT;
-		_pub_gpos.get().dead_reckoning = !_canEstimateXY && !_xyTimeout;
+		_pub_gpos.get().terrain_alt = _altHome - xLP(X_tz);
+		_pub_gpos.get().terrain_alt_valid = _validTZ;
+		_pub_gpos.get().dead_reckoning = !_validXY && !_xyTimeout;
 		_pub_gpos.get().pressure_alt = _sub_sensor.get().baro_alt_meter[0];
 		_pub_gpos.update();
 	}
@@ -646,23 +748,95 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 void BlockLocalPositionEstimator::initP()
 {
 	_P.setZero();
-	_P(X_x, X_x) = 1;
-	_P(X_y, X_y) = 1;
-	_P(X_z, X_z) = 1;
+	_P(X_x, X_x) = 2 * EST_STDDEV_XY_VALID; // initialize to twice valid condition
+	_P(X_y, X_y) = 2 * EST_STDDEV_XY_VALID;
+	_P(X_z, X_z) = 2 * EST_STDDEV_Z_VALID;
 	_P(X_vx, X_vx) = 1;
 	_P(X_vy, X_vy) = 1;
 	_P(X_vz, X_vz) = 1;
 	_P(X_bx, X_bx) = 1e-6;
 	_P(X_by, X_by) = 1e-6;
 	_P(X_bz, X_bz) = 1e-6;
-	_P(X_tz, X_tz) = 1;
+	_P(X_tz, X_tz) = 2 * EST_STDDEV_TZ_VALID;
+}
+
+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::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 BlockLocalPositionEstimator::updateSSParams()
+{
+	// input noise covariance matrix
+	_R.setZero();
+	_R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
+	_R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
+	_R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get();
+
+	// process noise power matrix
+	_Q.setZero();
+	float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get();
+	float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get();
+	_Q(X_x, X_x) = pn_p_sq;
+	_Q(X_y, X_y) = pn_p_sq;
+	_Q(X_z, X_z) = pn_p_sq;
+	_Q(X_vx, X_vx) = pn_v_sq;
+	_Q(X_vy, X_vy) = pn_v_sq;
+	_Q(X_vz, X_vz) = pn_v_sq;
+
+	// technically, the noise is in the body frame,
+	// but the components are all the same, so
+	// ignoring for now
+	float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get();
+	_Q(X_bx, X_bx) = pn_b_sq;
+	_Q(X_by, X_by) = pn_b_sq;
+	_Q(X_bz, X_bz) = pn_b_sq;
+
+	// terrain random walk noise
+	float pn_t_sq = _pn_t_noise_density.get() * _pn_t_noise_density.get();
+	_Q(X_tz, X_tz) = pn_t_sq;
 }

 void BlockLocalPositionEstimator::predict()
 {
 	// if can't update anything, don't propagate
 	// state or covariance
-	if (!_canEstimateXY && !_canEstimateZ) { return; }
+	if (!_validXY && !_validZ) { return; }

 	if (_integrate.get() && _sub_att.get().R_valid) {
 		Matrix3f R_att(_sub_att.get().R);
@@ -674,86 +848,26 @@ void BlockLocalPositionEstimator::predict()
 		_u = Vector3f(0, 0, 0);
 	}

-	// dynamics matrix
-	Matrix<float, n_x, n_x>  A; // state 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;
-
-	// 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);
-
-	// input matrix
-	Matrix<float, n_x, n_u>  B; // input matrix
-	B.setZero();
-	B(X_vx, U_ax) = 1;
-	B(X_vy, U_ay) = 1;
-	B(X_vz, U_az) = 1;
-
-	// input noise covariance matrix
-	Matrix<float, n_u, n_u> R;
-	R.setZero();
-	R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
-	R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get();
-	R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get();
-
-	// process noise power matrix
-	Matrix<float, n_x, n_x>  Q;
-	Q.setZero();
-	float pn_p_sq = _pn_p_noise_density.get() * _pn_p_noise_density.get();
-	float pn_v_sq = _pn_v_noise_density.get() * _pn_v_noise_density.get();
-	Q(X_x, X_x) = pn_p_sq;
-	Q(X_y, X_y) = pn_p_sq;
-	Q(X_z, X_z) = pn_p_sq;
-	Q(X_vx, X_vx) = pn_v_sq;
-	Q(X_vy, X_vy) = pn_v_sq;
-	Q(X_vz, X_vz) = pn_v_sq;
-
-	// technically, the noise is in the body frame,
-	// but the components are all the same, so
-	// ignoring for now
-	float pn_b_sq = _pn_b_noise_density.get() * _pn_b_noise_density.get();
-	Q(X_bx, X_bx) = pn_b_sq;
-	Q(X_by, X_by) = pn_b_sq;
-	Q(X_bz, X_bz) = pn_b_sq;
-
-	// terrain random walk noise
-	float pn_t_sq = _pn_t_noise_density.get() * _pn_t_noise_density.get();
-	Q(X_tz, X_tz) = pn_t_sq;
+	// update state space based on new states
+	updateSSStates();

 	// continuous time kalman filter prediction
-	Vector<float, n_x> dx = (A * _x + B * _u) * getDt();
-
-	// only predict for components we have
-	// valid measurements for
-	if (!_canEstimateXY) {
-		dx(X_x) = 0;
-		dx(X_y) = 0;
-		dx(X_vx) = 0;
-		dx(X_vy) = 0;
-	}
-
-	if (!_canEstimateZ) {
-		dx(X_z) = 0;
-		dx(X_vz) = 0;
-	}
+	// 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();
+	_P += (_A * _P + _P * _A.transpose() +
+	       _B * _R * _B.transpose() +
+	       _Q) * getDt();
+	_xLowPass.update(_x);
 }