void BlockLocalPositionEstimator::update()
{

	// wait for a sensor update, check for exit condition every 100 ms
	int ret = px4_poll(_polls, 3, 100);

	if (ret < 0) {
		/* poll error, count it in perf */
		perf_count(_err_perf);
		return;
	}

	uint64_t newTimeStamp = hrt_absolute_time();
	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
	_timeStamp = newTimeStamp;

	// set dt for all child blocks
	setDt(dt);

	// auto-detect connected rangefinders while not armed
	bool armedState = _sub_armed.get().armed;

	if (!armedState && (_sub_lidar == NULL || _sub_sonar == NULL)) {
		detectDistanceSensors();
	}

	// reset pos, vel, and terrain on arming
	if (!_lastArmedState && armedState) {

		// we just armed, we are at home position on the ground
		_x(X_x) = 0;
		_x(X_y) = 0;

		// the pressure altitude of home may have drifted, so we don't
		// reset z to zero

		// reset flow integral
		_flowX = 0;
		_flowY = 0;

		// we aren't moving, all velocities are zero
		_x(X_vx) = 0;
		_x(X_vy) = 0;
		_x(X_vz) = 0;

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

	// see which updates are available
	bool flowUpdated = _sub_flow.updated();
	bool paramsUpdated = _sub_param_update.updated();
	bool baroUpdated = _sub_sensor.updated();
	bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
	bool homeUpdated = _sub_home.updated();
	bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
	bool mocapUpdated = _sub_mocap.updated();
	bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
	bool sonarUpdated = (_sub_sonar != NULL) && _sub_sonar->updated();

	// get new data
	updateSubscriptions();

	// update parameters
	if (paramsUpdated) {
		updateParams();
		updateSSParams();
	}

	// update home position projection
	if (homeUpdated) {
		updateHome();
	}

	// is xy valid?
	bool xy_stddev_ok = sqrtf(math::max(_P(X_x, X_x), _P(X_y, X_y))) < _xy_pub_thresh.get();

	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 = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();

	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 = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();

	if (_validTZ) {
		if (!tz_stddev_ok) {
			_validTZ = false;
		}

	} else {
		if (tz_stddev_ok) {
			_validTZ = true;
		}
	}

	// timeouts
	if (_validXY) {
		_time_last_xy = _timeStamp;
	}

	if (_validZ) {
		_time_last_z = _timeStamp;
	}

	if (_validTZ) {
		_time_last_tz = _timeStamp;
	}

	// check timeouts
	checkTimeouts();

	// if we have no lat, lon initialize projection at 0,0
	if (_validXY && !_map_ref.init_done) {
		map_projection_init(&_map_ref,
				    _init_home_lat.get(),
				    _init_home_lon.get());
	}

	// reinitialize x if necessary
	bool reinit_x = false;

	for (int i = 0; i < n_x; i++) {
		// should we do a reinit
		// of sensors here?
		// don't want it to take too long
		if (!PX4_ISFINITE(_x(i))) {
			reinit_x = true;
			break;
		}
	}

	if (reinit_x) {
		for (int i = 0; i < n_x; i++) {
			_x(i) = 0;
		}

		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x");
	}

	// reinitialize P if necessary
	bool reinit_P = false;

	for (int i = 0; i < n_x; i++) {
		for (int j = 0; j < n_x; j++) {
			if (!PX4_ISFINITE(_P(i, j))) {
				reinit_P = true;
				break;
			}
		}

		if (reinit_P) { break; }
	}

	if (reinit_P) {
		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit P");
		initP();
	}

	// do prediction
	predict();

	// sensor corrections/ initializations
	if (gpsUpdated) {
		if (!_gpsInitialized) {
			gpsInit();

		} else {
			gpsCorrect();
		}
	}

	if (baroUpdated) {
		if (!_baroInitialized) {
			baroInit();

		} else {
			baroCorrect();
		}
	}

	if (lidarUpdated) {
		if (!_lidarInitialized) {
			lidarInit();

		} else {
			lidarCorrect();
		}
	}

	if (sonarUpdated) {
		if (!_sonarInitialized) {
			sonarInit();

		} else {
			sonarCorrect();
		}
	}

	if (flowUpdated) {
		if (!_flowInitialized) {
			flowInit();

		} else {
			perf_begin(_loop_perf);// TODO
			flowCorrect();
			//perf_count(_interval_perf);
			perf_end(_loop_perf);
		}
	}

	if (visionUpdated) {
		if (!_visionInitialized) {
			visionInit();

		} else {
			visionCorrect();
		}
	}

	if (mocapUpdated) {
		if (!_mocapInitialized) {
			mocapInit();

		} else {
			mocapCorrect();
		}
	}

	if (_altHomeInitialized) {
		// update all publications if possible
		publishLocalPos();
		publishEstimatorStatus();

		if (_validXY) {
			publishGlobalPos();
		}
	}

	// propagate delayed state, no matter what
	// if state is frozen, delayed state still
	// needs to be propagated with frozen state
	float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);

	if (_time_last_hist == 0 ||
	    (dt_hist > HIST_STEP)) {
		_tDelay.update(Scalar<uint64_t>(_timeStamp));
		_xDelay.update(_x);
		_time_last_hist = _timeStamp;
	}
}
void BlockLocalPositionEstimator::update()
{
	// wait for a sensor update, check for exit condition every 100 ms
	int ret = px4_poll(_polls, 3, 100);

	if (ret < 0) {
		return;
	}

	uint64_t newTimeStamp = hrt_absolute_time();
	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
	_timeStamp = newTimeStamp;

	// set dt for all child blocks
	setDt(dt);

	// auto-detect connected rangefinders while not armed
	bool armedState = _sub_armed.get().armed;

	if (!armedState && (_sub_lidar == nullptr || _sub_sonar == nullptr)) {
		// detect distance sensors
		for (int i = 0; i < N_DIST_SUBS; i++) {
			uORB::Subscription<distance_sensor_s> *s = _dist_subs[i];

			if (s == _sub_lidar || s == _sub_sonar) { continue; }

			if (s->updated()) {
				s->update();

				if (s->get().timestamp == 0) { continue; }

				if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER &&
				    s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
				    _sub_lidar == nullptr) {
					_sub_lidar = s;
					mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %i", msg_label, i);

				} else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND &&
					   s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
					   _sub_sonar == nullptr) {
					_sub_sonar = s;
					mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Sonar detected with ID %i", msg_label, i);
				}
			}
		}
	}

	// reset pos, vel, and terrain on arming

	// XXX this will be re-enabled for indoor use cases using a
	// selection param, but is really not helping outdoors
	// right now.

	// if (!_lastArmedState && armedState) {

	//      // we just armed, we are at origin on the ground
	//      _x(X_x) = 0;
	//      _x(X_y) = 0;
	//      // reset Z or not? _x(X_z) = 0;

	//      // we aren't moving, all velocities are zero
	//      _x(X_vx) = 0;
	//      _x(X_vy) = 0;
	//      _x(X_vz) = 0;

	//      // 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);
	//      _aglLowPass.setState(0);
	// }

	_lastArmedState = armedState;

	// see which updates are available
	bool paramsUpdated = _sub_param_update.updated();
	_baroUpdated = false;

	if ((_fusion.get() & FUSE_BARO) && _sub_sensor.updated()) {
		int32_t baro_timestamp_relative = _sub_sensor.get().baro_timestamp_relative;

		if (baro_timestamp_relative != _sub_sensor.get().RELATIVE_TIMESTAMP_INVALID) {
			uint64_t baro_timestamp = _sub_sensor.get().timestamp +	\
						  _sub_sensor.get().baro_timestamp_relative;

			if (baro_timestamp != _timeStampLastBaro) {
				_baroUpdated = true;
				_timeStampLastBaro = baro_timestamp;
			}
		}
	}

	_flowUpdated = (_fusion.get() & FUSE_FLOW) && _sub_flow.updated();
	_gpsUpdated = (_fusion.get() & FUSE_GPS) && _sub_gps.updated();
	_visionUpdated = (_fusion.get() & FUSE_VIS_POS) && _sub_vision_pos.updated();
	_mocapUpdated = _sub_mocap.updated();
	_lidarUpdated = (_sub_lidar != nullptr) && _sub_lidar->updated();
	_sonarUpdated = (_sub_sonar != nullptr) && _sub_sonar->updated();
	_landUpdated = landed() && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE);// throttle rate
	bool targetPositionUpdated = _sub_landing_target_pose.updated();

	// get new data
	updateSubscriptions();

	// update parameters
	if (paramsUpdated) {
		updateParams();
		updateSSParams();
	}

	// is xy valid?
	bool vxy_stddev_ok = false;

	if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get() * _vxy_pub_thresh.get()) {
		vxy_stddev_ok = true;
	}

	if (_estimatorInitialized & EST_XY) {
		// if valid and gps has timed out, set to not valid
		if (!vxy_stddev_ok && (_sensorTimeout & SENSOR_GPS)) {
			_estimatorInitialized &= ~EST_XY;
		}

	} else {
		if (vxy_stddev_ok) {
			if (!(_sensorTimeout & SENSOR_GPS)
			    || !(_sensorTimeout & SENSOR_FLOW)
			    || !(_sensorTimeout & SENSOR_VISION)
			    || !(_sensorTimeout & SENSOR_MOCAP)
			    || !(_sensorTimeout & SENSOR_LAND)
			    || !(_sensorTimeout & SENSOR_LAND_TARGET)
			   ) {
				_estimatorInitialized |= EST_XY;
			}
		}
	}

	// is z valid?
	bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();

	if (_estimatorInitialized & EST_Z) {
		// if valid and baro has timed out, set to not valid
		if (!z_stddev_ok && (_sensorTimeout & SENSOR_BARO)) {
			_estimatorInitialized &= ~EST_Z;
		}

	} else {
		if (z_stddev_ok) {
			_estimatorInitialized |= EST_Z;
		}
	}

	// is terrain valid?
	bool tz_stddev_ok = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();

	if (_estimatorInitialized & EST_TZ) {
		if (!tz_stddev_ok) {
			_estimatorInitialized &= ~EST_TZ;
		}

	} else {
		if (tz_stddev_ok) {
			_estimatorInitialized |= EST_TZ;
		}
	}

	// check timeouts
	checkTimeouts();

	// if we have no lat, lon initialize projection to LPE_LAT, LPE_LON parameters
	if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _fake_origin.get()) {
		map_projection_init(&_map_ref,
				    _init_origin_lat.get(),
				    _init_origin_lon.get());

		// set timestamp when origin was set to current time
		_time_origin = _timeStamp;

		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
					     double(_init_origin_lat.get()), double(_init_origin_lon.get()), double(_altOrigin));
	}

	// reinitialize x if necessary
	bool reinit_x = false;

	for (int i = 0; i < n_x; i++) {
		// should we do a reinit
		// of sensors here?
		// don't want it to take too long
		if (!PX4_ISFINITE(_x(i))) {
			reinit_x = true;
			mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x, x(%d) not finite", msg_label, i);
			break;
		}
	}

	if (reinit_x) {
		for (int i = 0; i < n_x; i++) {
			_x(i) = 0;
		}

		mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x", msg_label);
	}

	// force P symmetry and reinitialize P if necessary
	bool reinit_P = false;

	for (int i = 0; i < n_x; i++) {
		for (int j = 0; j <= i; j++) {
			if (!PX4_ISFINITE(_P(i, j))) {
				mavlink_and_console_log_info(&mavlink_log_pub,
							     "%sreinit P (%d, %d) not finite", msg_label, i, j);
				reinit_P = true;
			}

			if (i == j) {
				// make sure diagonal elements are positive
				if (_P(i, i) <= 0) {
					mavlink_and_console_log_info(&mavlink_log_pub,
								     "%sreinit P (%d, %d) negative", msg_label, i, j);
					reinit_P = true;
				}

			} else {
				// copy elememnt from upper triangle to force
				// symmetry
				_P(j, i) = _P(i, j);
			}

			if (reinit_P) { break; }
		}

		if (reinit_P) { break; }
	}

	if (reinit_P) {
		initP();
	}

	// do prediction
	predict();

	// sensor corrections/ initializations
	if (_gpsUpdated) {
		if (_sensorTimeout & SENSOR_GPS) {
			gpsInit();

		} else {
			gpsCorrect();
		}
	}

	if (_baroUpdated) {
		if (_sensorTimeout & SENSOR_BARO) {
			baroInit();

		} else {
			baroCorrect();
		}
	}

	if (_lidarUpdated) {
		if (_sensorTimeout & SENSOR_LIDAR) {
			lidarInit();

		} else {
			lidarCorrect();
		}
	}

	if (_sonarUpdated) {
		if (_sensorTimeout & SENSOR_SONAR) {
			sonarInit();

		} else {
			sonarCorrect();
		}
	}

	if (_flowUpdated) {
		if (_sensorTimeout & SENSOR_FLOW) {
			flowInit();

		} else {
			flowCorrect();
		}
	}

	if (_visionUpdated) {
		if (_sensorTimeout & SENSOR_VISION) {
			visionInit();

		} else {
			visionCorrect();
		}
	}

	if (_mocapUpdated) {
		if (_sensorTimeout & SENSOR_MOCAP) {
			mocapInit();

		} else {
			mocapCorrect();
		}
	}

	if (_landUpdated) {
		if (_sensorTimeout & SENSOR_LAND) {
			landInit();

		} else {
			landCorrect();
		}
	}

	if (targetPositionUpdated) {
		if (_sensorTimeout & SENSOR_LAND_TARGET) {
			landingTargetInit();

		} else {
			landingTargetCorrect();
		}
	}

	if (_altOriginInitialized) {
		// update all publications if possible
		publishLocalPos();
		publishEstimatorStatus();
		_pub_innov.get().timestamp = _timeStamp;
		_pub_innov.update();

		if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _fake_origin.get())) {
			publishGlobalPos();
		}
	}

	// propagate delayed state, no matter what
	// if state is frozen, delayed state still
	// needs to be propagated with frozen state
	float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);

	if (_time_last_hist == 0 ||
	    (dt_hist > HIST_STEP)) {
		_tDelay.update(Scalar<uint64_t>(_timeStamp));
		_xDelay.update(_x);
		_time_last_hist = _timeStamp;
	}
}
void BlockLocalPositionEstimator::update()
{

	// wait for a sensor update, check for exit condition every 100 ms
	int ret = px4_poll(_polls, 3, 100);

	if (ret < 0) {
		/* poll error, count it in perf */
		perf_count(_err_perf);
		return;
	}

	uint64_t newTimeStamp = hrt_absolute_time();
	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
	_timeStamp = newTimeStamp;

	// set dt for all child blocks
	setDt(dt);

	// auto-detect connected rangefinders while not armed
	bool armedState = _sub_armed.get().armed;

	if (!armedState && (_sub_lidar == NULL || _sub_sonar == NULL)) {
		detectDistanceSensors();
	}

	// reset pos, vel, and terrain on arming

	// XXX this will be re-enabled for indoor use cases using a
	// selection param, but is really not helping outdoors
	// right now.

	// if (!_lastArmedState && armedState) {

	// 	// we just armed, we are at origin on the ground
	// 	_x(X_x) = 0;
	// 	_x(X_y) = 0;
	// 	// reset Z or not? _x(X_z) = 0;

	// 	// we aren't moving, all velocities are zero
	// 	_x(X_vx) = 0;
	// 	_x(X_vy) = 0;
	// 	_x(X_vz) = 0;

	// 	// 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);
	// 	_aglLowPass.setState(0);
	// }

	_lastArmedState = armedState;

	// see which updates are available
	bool flowUpdated = _sub_flow.updated();
	bool paramsUpdated = _sub_param_update.updated();
	bool baroUpdated = _sub_sensor.updated();
	bool gpsUpdated = _gps_on.get() && _sub_gps.updated();
	bool visionUpdated = _vision_on.get() && _sub_vision_pos.updated();
	bool mocapUpdated = _sub_mocap.updated();
	bool lidarUpdated = (_sub_lidar != NULL) && _sub_lidar->updated();
	bool sonarUpdated = (_sub_sonar != NULL) && _sub_sonar->updated();
	bool landUpdated = (
				   (_sub_land.get().landed ||
				    ((!_sub_armed.get().armed) && (!_sub_land.get().freefall)))
				   && (!(_lidarInitialized || _mocapInitialized || _visionInitialized || _sonarInitialized))
				   && ((_timeStamp - _time_last_land) > 1.0e6f / LAND_RATE));

	// get new data
	updateSubscriptions();

	// update parameters
	if (paramsUpdated) {
		updateParams();
		updateSSParams();
	}

	// is xy valid?
	bool vxy_stddev_ok = false;

	if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get()*_vxy_pub_thresh.get()) {
		vxy_stddev_ok = true;
	}

	if (_validXY) {
		// if valid and gps has timed out, set to not valid
		if (!vxy_stddev_ok && !_gpsInitialized) {
			_validXY = false;
		}

	} else {
		if (vxy_stddev_ok) {
			if (_flowInitialized || _gpsInitialized || _visionInitialized || _mocapInitialized) {
				_validXY = true;
			}
		}
	}

	// is z valid?
	bool z_stddev_ok = sqrtf(_P(X_z, X_z)) < _z_pub_thresh.get();

	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 = sqrtf(_P(X_tz, X_tz)) < _z_pub_thresh.get();

	if (_validTZ) {
		if (!tz_stddev_ok) {
			_validTZ = false;
		}

	} else {
		if (tz_stddev_ok) {
			_validTZ = true;
		}
	}

	// timeouts
	if (_validXY) {
		_time_last_xy = _timeStamp;
	}

	if (_validZ) {
		_time_last_z = _timeStamp;
	}

	if (_validTZ) {
		_time_last_tz = _timeStamp;
	}

	// check timeouts
	checkTimeouts();

	// if we have no lat, lon initialize projection at 0,0
	if (_validXY && !_map_ref.init_done) {
		map_projection_init(&_map_ref,
				    _init_origin_lat.get(),
				    _init_origin_lon.get());
	}

	// reinitialize x if necessary
	bool reinit_x = false;

	for (int i = 0; i < n_x; i++) {
		// should we do a reinit
		// of sensors here?
		// don't want it to take too long
		if (!PX4_ISFINITE(_x(i))) {
			reinit_x = true;
			mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x, x(%d) not finite", i);
			break;
		}
	}

	if (reinit_x) {
		for (int i = 0; i < n_x; i++) {
			_x(i) = 0;
		}

		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit x");
	}

	// force P symmetry and reinitialize P if necessary
	bool reinit_P = false;

	for (int i = 0; i < n_x; i++) {
		for (int j = 0; j <= i; j++) {
			if (!PX4_ISFINITE(_P(i, j))) {
				reinit_P = true;
			}

			if (i == j) {
				// make sure diagonal elements are positive
				if (_P(i, i) <= 0) {
					reinit_P = true;
				}

			} else {
				// copy elememnt from upper triangle to force
				// symmetry
				_P(j, i) = _P(i, j);
			}

			if (reinit_P) { break; }
		}

		if (reinit_P) { break; }
	}

	if (reinit_P) {
		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] reinit P");
		initP();
	}

	// do prediction
	predict();

	// sensor corrections/ initializations
	if (gpsUpdated) {
		if (!_gpsInitialized) {
			gpsInit();

		} else {
			gpsCorrect();
		}
	}

	if (baroUpdated) {
		if (!_baroInitialized) {
			baroInit();

		} else {
			baroCorrect();
		}
	}

	if (lidarUpdated) {
		if (!_lidarInitialized) {
			lidarInit();

		} else {
			lidarCorrect();
		}
	}

	if (sonarUpdated) {
		if (!_sonarInitialized) {
			sonarInit();

		} else {
			sonarCorrect();
		}
	}

	if (flowUpdated) {
		if (!_flowInitialized) {
			flowInit();

		} else {
			perf_begin(_loop_perf);// TODO
			flowCorrect();
			//perf_count(_interval_perf);
			perf_end(_loop_perf);
		}
	}

	if (visionUpdated) {
		if (!_visionInitialized) {
			visionInit();

		} else {
			visionCorrect();
		}
	}

	if (mocapUpdated) {
		if (!_mocapInitialized) {
			mocapInit();

		} else {
			mocapCorrect();
		}
	}

	if (landUpdated) {
		if (!_landInitialized) {
			landInit();

		} else {
			landCorrect();
		}
	}

	if (_altOriginInitialized) {
		// update all publications if possible
		publishLocalPos();
		publishEstimatorStatus();
		_pub_innov.update();

		if (_validXY) {
			publishGlobalPos();
		}
	}

	// propagate delayed state, no matter what
	// if state is frozen, delayed state still
	// needs to be propagated with frozen state
	float dt_hist = 1.0e-6f * (_timeStamp - _time_last_hist);

	if (_time_last_hist == 0 ||
	    (dt_hist > HIST_STEP)) {
		_tDelay.update(Scalar<uint64_t>(_timeStamp));
		_xDelay.update(_x);
		_time_last_hist = _timeStamp;
	}
}