From e4a38a3e787b75d84c19b2f958620950dcc7564c Mon Sep 17 00:00:00 2001
From: Kenneth Bogert <kbogert@uga.edu>
Date: Wed, 17 Jun 2015 18:32:11 -0400
Subject: [PATCH] Improve scale estimation calculation slightly

By taking into account the increase in the altd reading
when the drone is pitching or rolling, this patch increases the
accuracy of the scale estimation.  It has a greater effect when
the drone is accellerating quickly and is a few meters off the ground.
---
 src/stateestimation/PTAMWrapper.cpp | 15 +++++++++++----
 src/stateestimation/Predictor.cpp   | 24 +++++++++++++++---------
 2 files changed, 26 insertions(+), 13 deletions(-)

diff --git a/src/stateestimation/PTAMWrapper.cpp b/src/stateestimation/PTAMWrapper.cpp
index 30fb118..9fbbbed 100644
--- a/src/stateestimation/PTAMWrapper.cpp
+++ b/src/stateestimation/PTAMWrapper.cpp
@@ -775,7 +775,7 @@ TooN::Vector<3> PTAMWrapper::evalNavQue(unsigned int from, unsigned int to, bool
 	pthread_mutex_lock(&navInfoQueueCS);
 	int skipped=0;
 	int used = 0;
-	int firstZ = 0;
+	double firstZ = 0;
 
 	float sum_first=0, num_first=0, sum_last=0, num_last=0;
 	int pressureAverageRange = 100;
@@ -822,8 +822,15 @@ TooN::Vector<3> PTAMWrapper::evalNavQue(unsigned int from, unsigned int to, bool
 			if(firstAdded == 0) 
 			{
 				firstAdded = frontStamp;
-				firstZ = cur->altd;
-				predIMUOnlyForScale->z = firstZ*0.001;	// avoid height check initially!
+				firstZ = (cur->altd * 0.001) / sqrt(1.0 + (tan(cur->rotX * 3.14159268 / 180)*tan(cur->rotX * 3.14159268 / 180)) \
+					+ (tan(cur->rotY * 3.14159268 / 180)*tan(cur->rotY * 3.14159268 / 180)) );
+
+				if (! std::isfinite(firstZ))
+					firstZ = cur->altd * 0.001;
+
+
+				predIMUOnlyForScale->z = firstZ;	// avoid height check initially!
+
 			}
 			lastAdded = frontStamp;
 			// add
@@ -837,7 +844,7 @@ TooN::Vector<3> PTAMWrapper::evalNavQue(unsigned int from, unsigned int to, bool
 
 	}
 	//printf("QueEval: before: %i; skipped: %i, used: %i, left: %i\n", totSize, skipped, used, navInfoQueue.size());
-	predIMUOnlyForScale->z -= firstZ*0.001;	// make height to height-diff
+	predIMUOnlyForScale->z -= firstZ;	// make height to height-diff
 
 	*zCorrupted = predIMUOnlyForScale->zCorrupted;
 	*allCorrupted = abs(firstAdded - (int)from) + abs(lastAdded - (int)to) > 80;
diff --git a/src/stateestimation/Predictor.cpp b/src/stateestimation/Predictor.cpp
index 3e30984..a401887 100644
--- a/src/stateestimation/Predictor.cpp
+++ b/src/stateestimation/Predictor.cpp
@@ -123,15 +123,21 @@ void Predictor::predictOneStep(ardrone_autonomy::Navdata* nfo)
 	x += sin(yawRad)*dxDrone+cos(yawRad)*dyDrone;
 	y += cos(yawRad)*dxDrone-sin(yawRad)*dyDrone;
 
-	// height
-	if(abs(z - (double)nfo->altd*0.001) > 0.12)
-	{
-		if(std::abs(z - (double)nfo->altd*0.001) > abs(zCorruptedJump))
-			zCorruptedJump = z - (double)nfo->altd*0.001;
-		zCorrupted = true;
-	}
-
-	z = nfo->altd*0.001;
+	double alt = (nfo->altd*0.001) / sqrt(1.0 + (tan(nfo->rotX * 3.14159268 / 180)*tan(nfo->rotX * 3.14159268 / 180)) \
+		+ (tan(nfo->rotY * 3.14159268 / 180)*tan(nfo->rotY * 3.14159268 / 180)) );
+
+	if (! std::isfinite(alt))
+		alt = nfo->altd * 0.001;
+
+ 	// height
+	if(abs(z - alt) > 0.12)
+ 	{
+		if(std::abs(z - alt) > abs(zCorruptedJump))
+			zCorruptedJump = z - alt;
+ 		zCorrupted = true;
+ 	}
+
+	z = alt;
 
 	// angles
 	roll = nfo->rotX/1000.0;