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.

src/lib/controllib/blocks.cpp

@@ -84,6 +84,7 @@ float BlockLowPass::update(float input)
 	return getState();
 }
 
+
 float BlockHighPass::update(float input)
 {
 	float b = 2 * float(M_PI) * getFCut() * getDt();

src/lib/controllib/blocks.hpp

@@ -127,6 +127,45 @@ protected:
 	control::BlockParamFloat _fCut;
 };
 
+template<class Type, size_t M>
+class __EXPORT BlockLowPassVector: public Block
+{
+public:
+// methods
+	BlockLowPassVector(SuperBlock *parent,
+			   const char *name) :
+		Block(parent, name),
+		_state(),
+		_fCut(this, "") // only one parameter, no need to name
+	{
+		for (int i = 0; i < M; i++) {
+			_state(i) = 0.0f / 0.0f;
+		}
+	};
+	virtual ~BlockLowPassVector() {};
+	matrix::Vector<Type, M> update(const matrix::Matrix<Type, M, 1> &input)
+	{
+		for (int i = 0; i < M; i++) {
+			if (!PX4_ISFINITE(getState()(i))) {
+				setState(input);
+			}
+		}
+
+		float b = 2 * float(M_PI) * getFCut() * getDt();
+		float a = b / (1 + b);
+		setState(input * a + getState() * (1 - a));
+		return getState();
+	}
+// accessors
+	matrix::Vector<Type, M> getState() { return _state; }
+	float getFCut() { return _fCut.get(); }
+	void setState(const matrix::Vector<Type, M> &state) { _state = state; }
+private:
+// attributes
+	matrix::Vector<Type, M> _state;
+	control::BlockParamFloat _fCut;
+};
+
 /**
  * A high pass filter as described here:
  * http://en.wikipedia.org/wiki/High-pass_filter.