Create altitude_kf.h

Author: rblilja

altitude_kf.h

@@ -0,0 +1,89 @@
+/**
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <[email protected]> wrote this file. As long as you retain this notice you
+ * can do whatever you want with this stuff. If we meet some day, and you think
+ * this stuff is worth it, you can buy me a beer in return. - Robin Lilja
+ *
+ * @file altitude_kf.h
+ * @author Robin Lilja
+ * @date 23 Jul 2015
+ */
+
+#ifndef _ALTITUDE_KF_H_
+#define _ALTITUDE_KF_H_
+
+/**
+ * A linear Kalman filter estimator of altitude and vertical velocity.
+ */
+class Altitude_KF {
+
+public:
+
+	/**
+	 * Constructor.
+	 * @param Q_accel covariance of acceleration input signal (σ^2).
+	 * @param R_altitude covariance of the altitude measurement (σ^2).
+	 */
+	Altitude_KF(float Q_accel, float R_altitude) {
+
+		this->Q_accel = Q_accel;
+		this->R_altitude = R_altitude;
+
+		h = 0.0f;
+		v = 0.0f;
+	}
+
+	/**
+	 * Propagate the state.
+	 * @param acceleration vertical acceleration in Earth frame (positive in the zenith direction) [m/s^2].
+	 * @param dt update/sampling period [s].
+	 */
+	void propagate(float acceleration, const float dt);
+
+	/**
+	 * State correction update. Use this method if you use multiple sensors measuring the altitude.
+	 * @param altitude measurement of altitude in Earth frame (positive in the zenith direction) [m].
+	 * @param R_altitude covariance of the altitude measurement (σ^2).
+	 */
+	void update(float altitude, float R_altitude);
+
+	/**
+	 * State correction update.
+	 * @param altitude measurement of altitude in Earth frame (positive in the zenith direction) [m].
+	 */
+	void update(float altitude) { update(altitude, this->R_altitude); };
+
+	/**
+	 * Estimated vertical height or altitude in Earth frame (positive in the zenith direction) [m].
+	 */
+	float h;
+
+	/**
+	 * Estimated vertical velocity (positive in the zenith direction) [m/s].
+	 */
+	float v;
+
+	/**
+	 * Accelerometer covariance.
+	 */
+	float Q_accel;
+
+	/**
+	 * Altitude measurement covariance.
+	 */
+	float R_altitude;
+
+private:
+
+	/**
+	 * Predicted covariance matrix 'P'.
+	 */
+	float P[2][2] =
+	{
+		{ 1.0f,     0.0f },
+		{ 0.0f,     1.0f }
+	};
+
+};
+
+#endif /* _ALTITUDE_KF_H_ */