From ea259d358de64c650198e28743220584e8e8e11c Mon Sep 17 00:00:00 2001
From: CarlOlsson <carlolsson.co@gmail.com>
Date: Fri, 8 Mar 2019 14:43:02 +0100
Subject: [PATCH] EKF: initialize the newest attitude estimate to a valid
 quaternion

Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
---
 EKF/ekf.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp
index 03a00e28be..81ad70852e 100644
--- a/EKF/ekf.cpp
+++ b/EKF/ekf.cpp
@@ -60,6 +60,7 @@ bool Ekf::init(uint64_t timestamp)
 	_output_new.vel.setZero();
 	_output_new.pos.setZero();
 	_output_new.quat_nominal.setZero();
+	_output_new.quat_nominal(0) = 1.0f;
 
 	_delta_angle_corr.setZero();
 	_imu_down_sampled.delta_ang.setZero();