Fully implemented EKF.

Still needs to be debugged as the result is way off, but it is all
there.

1 files changed, 10 insertions, 0 deletions

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;