att_pos_estimator_ekf restored

5 files changed, 1254 insertions, 0 deletions

diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
new file mode 100644
index 000000000..5cf84542b
--- /dev/null
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -0,0 +1,812 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file KalmanNav.cpp
+ *
+ * Kalman filter navigation code
+ */
+
+#include <poll.h>
+
+#include "KalmanNav.hpp"
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+// constants
+// Titterton pg. 52
+static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
+static const float R0 = 6378137.0f; // earth radius, m
+static const float g0 = 9.806f; // standard gravitational accel. m/s^2
+static const int8_t ret_ok = 0; 		// no error in function
+static const int8_t ret_error = -1; 	// error occurred
+
+KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	// subscriptions
+	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
+	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
+	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
+	// publications
+	_pos(&getPublications(), ORB_ID(vehicle_global_position)),
+	_localPos(&getPublications(), ORB_ID(vehicle_local_position)),
+	_att(&getPublications(), ORB_ID(vehicle_attitude)),
+	// timestamps
+	_pubTimeStamp(hrt_absolute_time()),
+	_predictTimeStamp(hrt_absolute_time()),
+	_attTimeStamp(hrt_absolute_time()),
+	_outTimeStamp(hrt_absolute_time()),
+	// frame count
+	_navFrames(0),
+	// miss counts
+	_miss(0),
+	// accelerations
+	fN(0), fE(0), fD(0),
+	// state
+	phi(0), theta(0), psi(0),
+	vN(0), vE(0), vD(0),
+	lat(0), lon(0), alt(0),
+	lat0(0), lon0(0), alt0(0),
+	// parameters for ground station
+	_vGyro(this, "V_GYRO"),
+	_vAccel(this, "V_ACCEL"),
+	_rMag(this, "R_MAG"),
+	_rGpsVel(this, "R_GPS_VEL"),
+	_rGpsPos(this, "R_GPS_POS"),
+	_rGpsAlt(this, "R_GPS_ALT"),
+	_rPressAlt(this, "R_PRESS_ALT"),
+	_rAccel(this, "R_ACCEL"),
+	_magDip(this, "ENV_MAG_DIP"),
+	_magDec(this, "ENV_MAG_DEC"),
+	_g(this, "ENV_G"),
+	_faultPos(this, "FAULT_POS"),
+	_faultAtt(this, "FAULT_ATT"),
+	_attitudeInitialized(false),
+	_positionInitialized(false),
+	_attitudeInitCounter(0)
+{
+	using namespace math;
+
+	memset(&ref, 0, sizeof(ref));
+
+	F.zero();
+	G.zero();
+	V.zero();
+	HAtt.zero();
+	RAtt.zero();
+	HPos.zero();
+	RPos.zero();
+
+	// initial state covariance matrix
+	P0.identity();
+	P0 *= 0.01f;
+	P = P0;
+
+	// initial state
+	phi = 0.0f;
+	theta = 0.0f;
+	psi = 0.0f;
+	vN = 0.0f;
+	vE = 0.0f;
+	vD = 0.0f;
+	lat = 0.0f;
+	lon = 0.0f;
+	alt = 0.0f;
+
+	// initialize rotation quaternion with a single raw sensor measurement
+	_sensors.update();
+	q = init(
+		_sensors.accelerometer_m_s2[0],
+		_sensors.accelerometer_m_s2[1],
+		_sensors.accelerometer_m_s2[2],
+		_sensors.magnetometer_ga[0],
+		_sensors.magnetometer_ga[1],
+		_sensors.magnetometer_ga[2]);
+
+	// initialize dcm
+	C_nb = q.to_dcm();
+
+	// HPos is constant
+	HPos(0, 3) = 1.0f;
+	HPos(1, 4) = 1.0f;
+	HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
+	HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
+	HPos(4, 8) = 1.0f;
+	HPos(5, 8) = 1.0f;
+
+	// initialize all parameters
+	updateParams();
+}
+
+math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz)
+{
+    float initialRoll, initialPitch;
+    float cosRoll, sinRoll, cosPitch, sinPitch;
+    float magX, magY;
+    float initialHdg, cosHeading, sinHeading;
+
+    initialRoll = atan2(-ay, -az);
+    initialPitch = atan2(ax, -az);
+
+    cosRoll = cosf(initialRoll);
+    sinRoll = sinf(initialRoll);
+    cosPitch = cosf(initialPitch);
+    sinPitch = sinf(initialPitch);
+
+    magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
+
+    magY = my * cosRoll - mz * sinRoll;
+
+    initialHdg = atan2f(-magY, magX);
+
+    cosRoll = cosf(initialRoll * 0.5f);
+    sinRoll = sinf(initialRoll * 0.5f);
+
+    cosPitch = cosf(initialPitch * 0.5f);
+    sinPitch = sinf(initialPitch * 0.5f);
+
+    cosHeading = cosf(initialHdg * 0.5f);
+    sinHeading = sinf(initialHdg * 0.5f);
+
+    float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
+    float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
+    float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
+    float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
+
+    return math::Quaternion(q0, q1, q2, q3);
+
+}
+
+void KalmanNav::update()
+{
+	using namespace math;
+
+	struct pollfd fds[1];
+	fds[0].fd = _sensors.getHandle();
+	fds[0].events = POLLIN;
+
+	// poll for new data
+	int ret = poll(fds, 1, 1000);
+
+	if (ret < 0) {
+		// XXX this is seriously bad - should be an emergency
+		return;
+
+	} else if (ret == 0) { // timeout
+		return;
+	}
+
+	// get new timestamp
+	uint64_t newTimeStamp = hrt_absolute_time();
+
+	// check updated subscriptions
+	if (_param_update.updated()) updateParams();
+
+	bool gpsUpdate = _gps.updated();
+	bool sensorsUpdate = _sensors.updated();
+
+	// get new information from subscriptions
+	// this clears update flag
+	updateSubscriptions();
+
+	// initialize attitude when sensors online
+	if (!_attitudeInitialized && sensorsUpdate) {
+		if (correctAtt() == ret_ok) _attitudeInitCounter++;
+
+		if (_attitudeInitCounter > 100) {
+			warnx("initialized EKF attitude");
+			warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f",
+			       double(phi), double(theta), double(psi));
+			_attitudeInitialized = true;
+		}
+	}
+
+	// initialize position when gps received
+	if (!_positionInitialized &&
+	    _attitudeInitialized && // wait for attitude first
+	    gpsUpdate &&
+	    _gps.fix_type > 2
+	    //&& _gps.counter_pos_valid > 10
+	   ) {
+		vN = _gps.vel_n_m_s;
+		vE = _gps.vel_e_m_s;
+		vD = _gps.vel_d_m_s;
+		setLatDegE7(_gps.lat);
+		setLonDegE7(_gps.lon);
+		setAltE3(_gps.alt);
+		// set reference position for
+		// local position
+		lat0 = lat;
+		lon0 = lon;
+		alt0 = alt;
+		map_projection_init(&ref, lat0, lon0);
+		_positionInitialized = true;
+		warnx("initialized EKF state with GPS");
+		warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
+		       double(vN), double(vE), double(vD),
+		       lat, lon, double(alt));
+	}
+
+	// prediction step
+	// using sensors timestamp so we can account for packet lag
+	float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
+	//printf("dt: %15.10f\n", double(dt));
+	_predictTimeStamp = _sensors.timestamp;
+
+	// don't predict if time greater than a second
+	if (dt < 1.0f) {
+		predictState(dt);
+		predictStateCovariance(dt);
+		// count fast frames
+		_navFrames += 1;
+	}
+
+	// count times 100 Hz rate isn't met
+	if (dt > 0.01f) _miss++;
+
+	// gps correction step
+	if (_positionInitialized && gpsUpdate) {
+		correctPos();
+	}
+
+	// attitude correction step
+	if (_attitudeInitialized 								// initialized
+	    && sensorsUpdate 								// new data
+	    && _sensors.timestamp - _attTimeStamp > 1e6 / 50 	// 50 Hz
+	   ) {
+		_attTimeStamp = _sensors.timestamp;
+		correctAtt();
+	}
+
+	// publication
+	if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
+		_pubTimeStamp = newTimeStamp;
+
+		updatePublications();
+	}
+
+	// output
+	if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
+		_outTimeStamp = newTimeStamp;
+		//printf("nav: %4d Hz, miss #: %4d\n",
+		//       _navFrames / 10, _miss / 10);
+		_navFrames = 0;
+		_miss = 0;
+	}
+}
+
+void KalmanNav::updatePublications()
+{
+	using namespace math;
+
+	// global position publication
+	_pos.timestamp = _pubTimeStamp;
+	_pos.time_gps_usec = _gps.timestamp_position;
+	_pos.lat = lat * M_RAD_TO_DEG;
+	_pos.lon = lon * M_RAD_TO_DEG;
+	_pos.alt = float(alt);
+	_pos.vel_n = vN;
+	_pos.vel_e = vE;
+	_pos.vel_d = vD;
+	_pos.yaw = psi;
+
+	// local position publication
+	float x;
+	float y;
+	bool landed = alt < (alt0 + 0.1); // XXX improve?
+	map_projection_project(&ref, lat, lon, &x, &y);
+	_localPos.timestamp = _pubTimeStamp;
+	_localPos.xy_valid = true;
+	_localPos.z_valid = true;
+	_localPos.v_xy_valid = true;
+	_localPos.v_z_valid = true;
+	_localPos.x = x;
+	_localPos.y = y;
+	_localPos.z = alt0 - alt;
+	_localPos.vx = vN;
+	_localPos.vy = vE;
+	_localPos.vz = vD;
+	_localPos.yaw = psi;
+	_localPos.xy_global = true;
+	_localPos.z_global = true;
+	_localPos.ref_timestamp = _pubTimeStamp;
+	_localPos.ref_lat = lat * M_RAD_TO_DEG;
+	_localPos.ref_lon = lon * M_RAD_TO_DEG;
+	_localPos.ref_alt = 0;
+	_localPos.landed = landed;
+
+	// attitude publication
+	_att.timestamp = _pubTimeStamp;
+	_att.roll = phi;
+	_att.pitch = theta;
+	_att.yaw = psi;
+	_att.rollspeed = _sensors.gyro_rad_s[0];
+	_att.pitchspeed = _sensors.gyro_rad_s[1];
+	_att.yawspeed = _sensors.gyro_rad_s[2];
+	// TODO, add gyro offsets to filter
+	_att.rate_offsets[0] = 0.0f;
+	_att.rate_offsets[1] = 0.0f;
+	_att.rate_offsets[2] = 0.0f;
+
+	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+			_att.R[i][j] = C_nb(i, j);
+
+	for (int i = 0; i < 4; i++) _att.q[i] = q(i);
+
+	_att.R_valid = true;
+	_att.q_valid = true;
+
+	// selectively update publications,
+	// do NOT call superblock do-all method
+	if (_positionInitialized) {
+		_pos.update();
+		_localPos.update();
+	}
+
+	if (_attitudeInitialized)
+		_att.update();
+}
+
+int KalmanNav::predictState(float dt)
+{
+	using namespace math;
+
+	// trig
+	float sinL = sinf(lat);
+	float cosL = cosf(lat);
+	float cosLSing = cosf(lat);
+
+	// prevent singularity
+	if (fabsf(cosLSing) < 0.01f) {
+		if (cosLSing > 0) cosLSing = 0.01;
+		else cosLSing = -0.01;
+	}
+
+	// attitude prediction
+	if (_attitudeInitialized) {
+		Vector<3> w(_sensors.gyro_rad_s);
+
+		// attitude
+		q = q + q.derivative(w) * dt;
+
+		// renormalize quaternion if needed
+		if (fabsf(q.length() - 1.0f) > 1e-4f) {
+			q.normalize();
+		}
+
+		// C_nb update
+		C_nb = q.to_dcm();
+
+		// euler update
+		Vector<3> euler = C_nb.to_euler();
+		phi = euler.data[0];
+		theta = euler.data[1];
+		psi = euler.data[2];
+
+		// specific acceleration in nav frame
+		Vector<3> accelB(_sensors.accelerometer_m_s2);
+		Vector<3> accelN = C_nb * accelB;
+		fN = accelN(0);
+		fE = accelN(1);
+		fD = accelN(2);
+	}
+
+	// position prediction
+	if (_positionInitialized) {
+		// neglects angular deflections in local gravity
+		// see Titerton pg. 70
+		float R = R0 + float(alt);
+		float LDot = vN / R;
+		float lDot = vE / (cosLSing * R);
+		float rotRate = 2 * omega + lDot;
+
+		// XXX position prediction using speed
+		float vNDot = fN - vE * rotRate * sinL +
+			      vD * LDot;
+		float vDDot = fD - vE * rotRate * cosL -
+			      vN * LDot + _g.get();
+		float vEDot = fE + vN * rotRate * sinL +
+			      vDDot * rotRate * cosL;
+
+		// rectangular integration
+		vN += vNDot * dt;
+		vE += vEDot * dt;
+		vD += vDDot * dt;
+		lat += double(LDot * dt);
+		lon += double(lDot * dt);
+		alt += double(-vD * dt);
+	}
+
+	return ret_ok;
+}
+
+int KalmanNav::predictStateCovariance(float dt)
+{
+	using namespace math;
+
+	// trig
+	float sinL = sinf(lat);
+	float cosL = cosf(lat);
+	float cosLSq = cosL * cosL;
+	float tanL = tanf(lat);
+
+	// prepare for matrix
+	float R = R0 + float(alt);
+	float RSq = R * R;
+
+	// F Matrix
+	// Titterton pg. 291
+
+	F(0, 1) = -(omega * sinL + vE * tanL / R);
+	F(0, 2) = vN / R;
+	F(0, 4) = 1.0f / R;
+	F(0, 6) = -omega * sinL;
+	F(0, 8) = -vE / RSq;
+
+	F(1, 0) = omega * sinL + vE * tanL / R;
+	F(1, 2) = omega * cosL + vE / R;
+	F(1, 3) = -1.0f / R;
+	F(1, 8) = vN / RSq;
+
+	F(2, 0) = -vN / R;
+	F(2, 1) = -omega * cosL - vE / R;
+	F(2, 4) = -tanL / R;
+	F(2, 6) = -omega * cosL - vE / (R * cosLSq);
+	F(2, 8) = vE * tanL / RSq;
+
+	F(3, 1) = -fD;
+	F(3, 2) = fE;
+	F(3, 3) = vD / R;
+	F(3, 4) = -2 * (omega * sinL + vE * tanL / R);
+	F(3, 5) = vN / R;
+	F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq));
+	F(3, 8) = (vE * vE * tanL - vN * vD) / RSq;
+
+	F(4, 0) = fD;
+	F(4, 2) = -fN;
+	F(4, 3) = 2 * omega * sinL + vE * tanL / R;
+	F(4, 4) = (vN * tanL + vD) / R;
+	F(4, 5) = 2 * omega * cosL + vE / R;
+	F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) +
+		  vN * vE / (R * cosLSq);
+	F(4, 8) = -vE * (vN * tanL + vD) / RSq;
+
+	F(5, 0) = -fE;
+	F(5, 1) = fN;
+	F(5, 3) = -2 * vN / R;
+	F(5, 4) = -2 * (omega * cosL + vE / R);
+	F(5, 6) = 2 * omega * vE * sinL;
+	F(5, 8) = (vN * vN + vE * vE) / RSq;
+
+	F(6, 3) = 1 / R;
+	F(6, 8) = -vN / RSq;
+
+	F(7, 4) = 1 / (R * cosL);
+	F(7, 6) = vE * tanL / (R * cosL);
+	F(7, 8) = -vE / (cosL * RSq);
+
+	F(8, 5) = -1;
+
+	// G Matrix
+	// Titterton pg. 291
+	G(0, 0) = -C_nb(0, 0);
+	G(0, 1) = -C_nb(0, 1);
+	G(0, 2) = -C_nb(0, 2);
+	G(1, 0) = -C_nb(1, 0);
+	G(1, 1) = -C_nb(1, 1);
+	G(1, 2) = -C_nb(1, 2);
+	G(2, 0) = -C_nb(2, 0);
+	G(2, 1) = -C_nb(2, 1);
+	G(2, 2) = -C_nb(2, 2);
+
+	G(3, 3) = C_nb(0, 0);
+	G(3, 4) = C_nb(0, 1);
+	G(3, 5) = C_nb(0, 2);
+	G(4, 3) = C_nb(1, 0);
+	G(4, 4) = C_nb(1, 1);
+	G(4, 5) = C_nb(1, 2);
+	G(5, 3) = C_nb(2, 0);
+	G(5, 4) = C_nb(2, 1);
+	G(5, 5) = C_nb(2, 2);
+
+	// continuous prediction equations
+	// for discrete time EKF
+	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
+	P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt;
+
+	return ret_ok;
+}
+
+int KalmanNav::correctAtt()
+{
+	using namespace math;
+
+	// trig
+	float cosPhi = cosf(phi);
+	float cosTheta = cosf(theta);
+	// float cosPsi = cosf(psi);
+	float sinPhi = sinf(phi);
+	float sinTheta = sinf(theta);
+	// float sinPsi = sinf(psi);
+
+	// mag predicted measurement
+	// choosing some typical magnetic field properties,
+	//  TODO dip/dec depend on lat/ lon/ time
+	//float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
+	float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
+
+	// compensate roll and pitch, but not yaw
+	// XXX take the vectors out of the C_nb matrix to avoid singularities
+	math::Matrix<3,3> C_rp;
+	C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed();
+
+	// mag measurement
+	Vector<3> magBody(_sensors.magnetometer_ga);
+
+	// transform to earth frame
+	Vector<3> magNav = C_rp * magBody;
+
+	// calculate error between estimate and measurement
+	// apply declination correction for true heading as well.
+	float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec;
+	if (yMag > M_PI_F) yMag -= 2*M_PI_F;
+	if (yMag < -M_PI_F) yMag += 2*M_PI_F;
+
+	// accel measurement
+	Vector<3> zAccel(_sensors.accelerometer_m_s2);
+	float accelMag = zAccel.length();
+	zAccel.normalize();
+
+	// ignore accel correction when accel mag not close to g
+	Matrix<4,4> RAttAdjust = RAtt;
+
+	bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
+
+	if (ignoreAccel) {
+		RAttAdjust(1, 1) = 1.0e10;
+		RAttAdjust(2, 2) = 1.0e10;
+		RAttAdjust(3, 3) = 1.0e10;
+
+	} else {
+		//printf("correcting attitude with accel\n");
+	}
+
+	// accel predicted measurement
+	Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized();
+
+	// calculate residual
+	Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2));
+
+	// HMag
+	HAtt(0, 2) = 1;
+
+	// HAccel
+	HAtt(1, 1) = cosTheta;
+	HAtt(2, 0) = -cosPhi * cosTheta;
+	HAtt(2, 1) = sinPhi * sinTheta;
+	HAtt(3, 0) = sinPhi * cosTheta;
+	HAtt(3, 1) = cosPhi * sinTheta;
+
+	// compute correction
+	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
+	Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance
+	Matrix<9, 4> K = P * HAtt.transposed() * S.inversed();
+	Vector<9> xCorrect = K * y;
+
+	// check correciton is sane
+	for (size_t i = 0; i < xCorrect.get_size(); i++) {
+		float val = xCorrect(i);
+
+		if (isnan(val) || isinf(val)) {
+			// abort correction and return
+			warnx("numerical failure in att correction");
+			// reset P matrix to P0
+			P = P0;
+			return ret_error;
+		}
+	}
+
+	// correct state
+	if (!ignoreAccel) {
+		phi += xCorrect(PHI);
+		theta += xCorrect(THETA);
+	}
+
+	psi += xCorrect(PSI);
+
+	// attitude also affects nav velocities
+	if (_positionInitialized) {
+		vN += xCorrect(VN);
+		vE += xCorrect(VE);
+		vD += xCorrect(VD);
+	}
+
+	// update state covariance
+	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
+	P = P - K * HAtt * P;
+
+	// fault detection
+	float beta = y * (S.inversed() * y);
+
+	if (beta > _faultAtt.get()) {
+		warnx("fault in attitude: beta = %8.4f", (double)beta);
+		warnx("y:"); y.print();
+	}
+
+	// update quaternions from euler
+	// angle correction
+	q.from_euler(phi, theta, psi);
+
+	return ret_ok;
+}
+
+int KalmanNav::correctPos()
+{
+	using namespace math;
+
+	// residual
+	Vector<6> y;
+	y(0) = _gps.vel_n_m_s - vN;
+	y(1) = _gps.vel_e_m_s - vE;
+	y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
+	y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG;
+	y(4) = _gps.alt / 1.0e3f - alt;
+	y(5) = _sensors.baro_alt_meter - alt;
+
+	// compute correction
+	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
+	Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance
+	Matrix<9,6> K = P * HPos.transposed() * S.inversed();
+	Vector<9> xCorrect = K * y;
+
+	// check correction is sane
+	for (size_t i = 0; i < xCorrect.get_size(); i++) {
+		float val = xCorrect(i);
+
+		if (!isfinite(val)) {
+			// abort correction and return
+			warnx("numerical failure in gps correction");
+			// fallback to GPS
+			vN = _gps.vel_n_m_s;
+			vE = _gps.vel_e_m_s;
+			vD = _gps.vel_d_m_s;
+			setLatDegE7(_gps.lat);
+			setLonDegE7(_gps.lon);
+			setAltE3(_gps.alt);
+			// reset P matrix to P0
+			P = P0;
+			return ret_error;
+		}
+	}
+
+	// correct state
+	vN += xCorrect(VN);
+	vE += xCorrect(VE);
+	vD += xCorrect(VD);
+	lat += double(xCorrect(LAT));
+	lon += double(xCorrect(LON));
+	alt += xCorrect(ALT);
+
+	// update state covariance
+	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
+	P = P - K * HPos * P;
+
+	// fault detetcion
+	float beta = y * (S.inversed() * y);
+
+	static int counter = 0;
+	if (beta > _faultPos.get() && (counter % 10 == 0)) {
+		warnx("fault in gps: beta = %8.4f", (double)beta);
+		warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f",
+		       double(y(0) / sqrtf(RPos(0, 0))),
+		       double(y(1) / sqrtf(RPos(1, 1))),
+		       double(y(2) / sqrtf(RPos(2, 2))),
+		       double(y(3) / sqrtf(RPos(3, 3))),
+		       double(y(4) / sqrtf(RPos(4, 4))),
+		       double(y(5) / sqrtf(RPos(5, 5))));
+	}
+	counter++;
+
+	return ret_ok;
+}
+
+void KalmanNav::updateParams()
+{
+	using namespace math;
+	using namespace control;
+	SuperBlock::updateParams();
+
+	// gyro noise
+	V(0, 0) = _vGyro.get();   // gyro x, rad/s
+	V(1, 1) = _vGyro.get();   // gyro y
+	V(2, 2) = _vGyro.get();   // gyro z
+
+	// accel noise
+	V(3, 3) = _vAccel.get();   // accel x, m/s^2
+	V(4, 4) = _vAccel.get();   // accel y
+	V(5, 5) = _vAccel.get();   // accel z
+
+	// magnetometer noise
+	float noiseMin = 1e-6f;
+	float noiseMagSq = _rMag.get() * _rMag.get();
+
+	if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
+
+	RAtt(0, 0) = noiseMagSq; // normalized direction
+
+	// accelerometer noise
+	float noiseAccelSq = _rAccel.get() * _rAccel.get();
+
+	// bound noise to prevent singularities
+	if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
+
+	RAtt(1, 1) = noiseAccelSq; // normalized direction
+	RAtt(2, 2) = noiseAccelSq;
+	RAtt(3, 3) = noiseAccelSq;
+
+	// gps noise
+	float R = R0 + float(alt);
+	float cosLSing = cosf(lat);
+
+	// prevent singularity
+	if (fabsf(cosLSing) < 0.01f) {
+		if (cosLSing > 0) cosLSing = 0.01;
+		else cosLSing = -0.01;
+	}
+
+	float noiseVel = _rGpsVel.get();
+	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
+	float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
+	float noiseGpsAlt = _rGpsAlt.get();
+	float noisePressAlt = _rPressAlt.get();
+
+	// bound noise to prevent singularities
+	if (noiseVel < noiseMin) noiseVel = noiseMin;
+
+	if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin;
+
+	if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
+
+	if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin;
+
+	if (noisePressAlt < noiseMin) noisePressAlt = noiseMin;
+
+	RPos(0, 0) = noiseVel * noiseVel; // vn
+	RPos(1, 1) = noiseVel * noiseVel; // ve
+	RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
+	RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
+	RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h
+	RPos(5, 5) = noisePressAlt * noisePressAlt; // h
+	// XXX, note that RPos depends on lat, so updateParams should
+	// be called if lat changes significantly
+}
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
new file mode 100644
index 000000000..24df153cb
--- /dev/null
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -0,0 +1,194 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file KalmanNav.hpp
+ *
+ * kalman filter navigation code
+ */
+
+#pragma once
+
+//#define MATRIX_ASSERT
+//#define VECTOR_ASSERT
+
+#include <nuttx/config.h>
+
+#include <mathlib/mathlib.h>
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+#include <lib/geo/geo.h>
+
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/parameter_update.h>
+
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+
+#include <drivers/drv_hrt.h>
+#include <poll.h>
+#include <unistd.h>
+
+/**
+ * Kalman filter navigation class
+ * http://en.wikipedia.org/wiki/Extended_Kalman_filter
+ * Discrete-time extended Kalman filter
+ */
+class KalmanNav : public control::SuperBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	KalmanNav(SuperBlock *parent, const char *name);
+
+	/**
+	 * Deconstuctor
+	 */
+
+	virtual ~KalmanNav() {};
+
+	math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz);
+
+	/**
+	 * The main callback function for the class
+	 */
+	void update();
+
+
+	/**
+	 * Publication update
+	 */
+	virtual void updatePublications();
+
+	/**
+	 * State prediction
+	 * Continuous, non-linear
+	 */
+	int predictState(float dt);
+
+	/**
+	 * State covariance prediction
+	 * Continuous, linear
+	 */
+	int predictStateCovariance(float dt);
+
+	/**
+	 * Attitude correction
+	 */
+	int correctAtt();
+
+	/**
+	 * Position correction
+	 */
+	int correctPos();
+
+	/**
+	 * Overloaded update parameters
+	 */
+	virtual void updateParams();
+protected:
+	// kalman filter
+	math::Matrix<9,9> F;             /**< Jacobian(f,x), where dx/dt = f(x,u) */
+	math::Matrix<9,6> G;             /**< noise shaping matrix for gyro/accel */
+	math::Matrix<9,9> P;             /**< state covariance matrix */
+	math::Matrix<9,9> P0;            /**< initial state covariance matrix */
+	math::Matrix<6,6> V;             /**< gyro/ accel noise matrix */
+	math::Matrix<4,9> HAtt;          /**< attitude measurement matrix */
+	math::Matrix<4,4> RAtt;          /**< attitude measurement noise matrix */
+	math::Matrix<6,9> HPos;          /**< position measurement jacobian matrix */
+	math::Matrix<6,6> RPos;          /**< position measurement noise matrix */
+	// attitude
+	math::Matrix<3,3> C_nb;             /**< direction cosine matrix from body to nav frame */
+	math::Quaternion q;         /**< quaternion from body to nav frame */
+	// subscriptions
+	uORB::Subscription<sensor_combined_s> _sensors;          /**< sensors sub. */
+	uORB::Subscription<vehicle_gps_position_s> _gps;         /**< gps sub. */
+	uORB::Subscription<parameter_update_s> _param_update;    /**< parameter update sub. */
+	// publications
+	uORB::Publication<vehicle_global_position_s> _pos;       /**< position pub. */
+	uORB::Publication<vehicle_local_position_s> _localPos;   /**< local position pub. */
+	uORB::Publication<vehicle_attitude_s> _att;              /**< attitude pub. */
+	// time stamps
+	uint64_t _pubTimeStamp;     /**< output data publication time stamp */
+	uint64_t _predictTimeStamp; /**< prediction time stamp */
+	uint64_t _attTimeStamp;     /**< attitude correction time stamp */
+	uint64_t _outTimeStamp;     /**< output time stamp */
+	// frame count
+	uint16_t _navFrames;        /**< navigation frames completed in output cycle */
+	// miss counts
+	uint16_t _miss;         	/**< number of times fast prediction loop missed */
+	// accelerations
+	float fN, fE, fD;           /**< navigation frame acceleration */
+	// states
+	enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};  /**< state enumeration */
+	float phi, theta, psi;                  /**< 3-2-1 euler angles */
+	float vN, vE, vD;                       /**< navigation velocity, m/s */
+	double lat, lon;                   	/**< lat, lon radians */
+	// parameters
+	float alt;                   		/**< altitude, meters */
+	double lat0, lon0;                   	/**< reference latitude and longitude */
+	struct map_projection_reference_s ref;	/**< local projection reference */
+	float alt0;                   		/**<  refeerence altitude (ground height) */
+	control::BlockParamFloat _vGyro;      /**< gyro process noise */
+	control::BlockParamFloat _vAccel;     /**< accelerometer process noise  */
+	control::BlockParamFloat _rMag;       /**< magnetometer measurement noise  */
+	control::BlockParamFloat _rGpsVel;    /**< gps velocity measurement noise */
+	control::BlockParamFloat _rGpsPos;    /**< gps position measurement noise */
+	control::BlockParamFloat _rGpsAlt;    /**< gps altitude measurement noise */
+	control::BlockParamFloat _rPressAlt;  /**< press altitude measurement noise */
+	control::BlockParamFloat _rAccel;     /**< accelerometer measurement noise */
+	control::BlockParamFloat _magDip;     /**< magnetic inclination with level */
+	control::BlockParamFloat _magDec;     /**< magnetic declination, clockwise rotation */
+	control::BlockParamFloat _g;          /**< gravitational constant */
+	control::BlockParamFloat _faultPos;   /**< fault detection threshold for position */
+	control::BlockParamFloat _faultAtt;   /**< fault detection threshold for attitude */
+	// status
+	bool _attitudeInitialized;
+	bool _positionInitialized;
+	uint16_t _attitudeInitCounter;
+	// accessors
+	int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
+	void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
+	int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
+	void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; }
+	int32_t getAltE3() { return int32_t(alt * 1.0e3); }
+	void setAltE3(int32_t val) { alt = double(val) / 1.0e3; }
+};
diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
new file mode 100644
index 000000000..3d20d4d2d
--- /dev/null
+++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
@@ -0,0 +1,157 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	James Goppert
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file kalman_main.cpp
+ * Combined attitude / position estimator.
+ *
+ * @author James Goppert
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <math.h>
+#include "KalmanNav.hpp"
+
+static bool thread_should_exit = false;     /**< Deamon exit flag */
+static bool thread_running = false;     /**< Deamon status flag */
+static int daemon_task;             /**< Handle of deamon task / thread */
+
+/**
+ * Deamon management function.
+ */
+extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int kalman_demo_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p <additional params>]");
+	exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int att_pos_estimator_ekf_main(int argc, char *argv[])
+{
+
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+
+		daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 30,
+					 8192,
+					 kalman_demo_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running\n");
+			exit(0);
+
+		} else {
+			warnx("not started\n");
+			exit(1);
+		}
+
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int kalman_demo_thread_main(int argc, char *argv[])
+{
+
+	warnx("starting");
+
+	using namespace math;
+
+	thread_running = true;
+
+	KalmanNav nav(NULL, "KF");
+
+	while (!thread_should_exit) {
+		nav.update();
+	}
+
+	warnx("exiting.");
+
+	thread_running = false;
+
+	return 0;
+}
diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/src/modules/att_pos_estimator_ekf/module.mk
new file mode 100644
index 000000000..8d4a40d95
--- /dev/null
+++ b/src/modules/att_pos_estimator_ekf/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Full attitude / position Extended Kalman Filter
+#
+
+MODULE_COMMAND	= att_pos_estimator_ekf
+
+SRCS		= kalman_main.cpp \
+		  KalmanNav.cpp \
+		  params.c
diff --git a/src/modules/att_pos_estimator_ekf/params.c b/src/modules/att_pos_estimator_ekf/params.c
new file mode 100644
index 000000000..4af5edead
--- /dev/null
+++ b/src/modules/att_pos_estimator_ekf/params.c
@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <systemlib/param/param.h>
+
+/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
+PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f);
+PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
+PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f);
+PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f);
+PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
+PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
+PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
+PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
+PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
+PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);