From b2cba6dc96c22f35fcf0df75527868cc36e93760 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Sun, 26 Mar 2017 16:33:20 -0500
Subject: Fully implemented EKF.

Still needs to be debugged as the result is way off, but it is all
there.
---
 src/main.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

(limited to 'src/main.cpp')

diff --git a/src/main.cpp b/src/main.cpp
index bcf6b37..85bf09f 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -47,6 +47,16 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q)
 {
     if (seenutm && seenpva && seencov) {
         std::vector<measurement_t> z;
+
+        Vector3d tip;
+        tip << 0.6*3.43, 0, -0.34;
+        tip = q._transformVector(tip);
+
+        measurement_t height;
+        height.z_type = HEIGHT;
+        height.height = tip[2];
+        z.push_back(height);
+
         int id,sx,sy,rx,ry;
         strcat(msg.image_names[0],".txt");
         FILE *fin;
-- 
cgit v1.1