use kinematics module from mbed.org

1 files changed, 170 insertions, 139 deletions

diff --git a/ctrl/kinematics.vala b/ctrl/kinematics.vala
index 52c4921..47a41bc 100644
--- a/ctrl/kinematics.vala
+++ b/ctrl/kinematics.vala
@@ -1,152 +1,183 @@
-/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
- * Copyright 2012, Ted Carancho
+/* Copyright 2010, ARM Limited
+ * Original C++ Code written by Aaron Berk
+ * Ported to Vala by Sebastian Reichel <sre@ring0.de>
  *
- * Ported from AeroQuad
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to
+ * deal in the Software without restriction, including without limitation the
+ * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+ * sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
  *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
  *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
  */
 
 public class Kinematics {
-	/* quaternion elements representing the estimated orientation */
-	double q0;
-	double q1;
-	double q2;
-	double q3;
-
-	/* scaled integral error */
-	double exInt = 0.0;
-	double eyInt = 0.0;
-	double ezInt = 0.0;
-
-	/* proportional gain governs rate of convergence to accelerometer */
-	double kpAcc = 0.2;
-
-	/* integral gain governs rate of convergence of gyroscope biases */
-	double kiAcc = 0.0005;
-
-	/* proportional gain governs rate of convergence to magnetometer */
-	double kpMag = 2.0;
-	/* integral gain governs rate of convergence of gyroscope biases */
-	double kiMag = 0.005;
-
-	/* half the sample period*/
-	double halfT = 0.0;
-
-	public double[] kinematicsAngle = new double[3];
-	public double[] correctedRateVector = new double[3];
-	public double[] gyroAngle = new double[2];
-
-	public Kinematics(double hdgX, double hdgY) {
-		for (var axis = AXIS.X; axis <= AXIS.Z; axis+=1)
-			kinematicsAngle[axis] = 0;
-		for (var axis = AXIS.X; axis <= AXIS.Y; axis+=1)
-			gyroAngle[axis] = 0;
-
-		double hdg = Math.atan2(hdgY, hdgX);
-		q0 = Math.cos(hdg/2);
-		q1 = 0;
-		q2 = 0;
-		q3 = Math.sin(hdg/2);
+	double[] result = new double[3];
+    bool firstUpdate;
+
+    /* Quaternion orientation of earth frame relative to auxiliary frame. */
+    double AEq_1;
+    double AEq_2;
+    double AEq_3;
+    double AEq_4;
+
+    /* Estimated orientation quaternion elements with initial conditions. */
+    double SEq_1;
+    double SEq_2;
+    double SEq_3;
+    double SEq_4;
+
+	/* filter tuning constant */
+	double beta;
+
+	uint i = 0;
+
+	public Kinematics() {
+		reset();
+
+		var gyroMeasError = 10.0;
+		beta = Math.sqrt(3.0 / 4.0) * (Math.PI * (gyroMeasError / 180.0));
 	}
 
 	public void update(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz, double time) {
-		double norm;
-		double hx, hy, hz, bx, bz;
-		double vx, vy, vz, wx, wy, wz;
-		double exAcc, eyAcc, ezAcc;
-		double exMag, eyMag, ezMag;
-		double q0i, q1i, q2i, q3i;
-
-		log("Kinematics", LogLevelFlags.LEVEL_DEBUG, @"sensors: GYRO: $gx $gy $gz, ACCEL: $ax $ay $az, COMPASS: $mx $my $mz");
-
-		halfT = time/2;
-
-		/* normalise the accelerometer measurements */
-		norm = Math.sqrt(ax*ax + ay*ay + az*az);
-		ax = ax / norm;
-		ay = ay / norm;
-		az = az / norm;
-
-		/* normalise the magnometer measurements */
-		norm = Math.sqrt(mx*mx + my*my + mz*mz);
-		mx = mx / norm;
-		my = my / norm;
-		mz = mz / norm;
-
-		/* compute reference direction of flux */
-		hx = mx * 2 * (0.5 - q2 * q2 - q3 * q3) + my * 2 * (q1 * q2 - q0 * q3)       + mz * 2 * (q1 * q3 + q0 * q2);
-		hy = mx * 2 * (q1 * q2 + q0 * q3)       + my * 2 * (0.5 - q1 * q1 - q3 * q3) + mz * 2 * (q2 * q3 - q0 * q1);
-		hz = mx * 2 * (q1 * q3 - q0 * q2)       + my * 2 * (q2 * q3 + q0 * q1)       + mz * 2 * (0.5 - q1 * q1 - q2 * q2);
-
-		bx = Math.sqrt((hx*hx) + (hy*hy));
-		bz = hz;
-
-		/* estimate direction of gravity and flux (v and w) */
-		vx = 2 *(q1 * q3 - q0 * q2);
-		vy = 2 *(q0 * q1 + q2 * q3);
-		vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
-
-		wx = bx * 2*(0.5 - q2 * q2 - q3 * q3) + bz * 2 * (q1 * q3 - q0 * q2);
-		wy = bx * 2*(q1 * q2 - q0 * q3)       + bz * 2 * (q0 * q1 + q2 * q3);
-		wz = bx * 2*(q0 * q2 + q1 * q3)       + bz * 2 * (0.5 - q1 * q1 - q2 * q2);
-
-		/* error is sum of cross product between reference direction of fields and direction measured by sensors */
-		exAcc = (vy*az - vz*ay);
-		eyAcc = (vz*ax - vx*az);
-		ezAcc = (vx*ay - vy*ax);
-
-		exMag = (my*wz - mz*wy);
-		eyMag = (mz*wx - mx*wz);
-		ezMag = (mx*wy - my*wx);
-
-		/* integral error scaled integral gain */
-		exInt = exInt + exAcc*kiAcc + exMag*kiMag;
-		eyInt = eyInt + eyAcc*kiAcc + eyMag*kiMag;
-		ezInt = ezInt + ezAcc*kiAcc + ezMag*kiMag;
-
-		/* adjusted gyroscope measurements */
-		correctedRateVector[AXIS.X] = gx = gx + exAcc*kpAcc + exMag*kpMag + exInt;
-		correctedRateVector[AXIS.Y] = gy = gy + eyAcc*kpAcc + eyMag*kpMag + eyInt;
-		correctedRateVector[AXIS.Z] = gz = gz + ezAcc*kpAcc + ezMag*kpMag + ezInt;
-
-		/* integrate quaternion rate and normalise */
-		q0i = (-q1*gx - q2*gy - q3*gz) * halfT;
-		q1i = ( q0*gx + q2*gz - q3*gy) * halfT;
-		q2i = ( q0*gy - q1*gz + q3*gx) * halfT;
-		q3i = ( q0*gz + q1*gy - q2*gx) * halfT;
-		q0 += q0i;
-		q1 += q1i;
-		q2 += q2i;
-		q3 += q3i;
+		//var deltat = time/1000000.0;
+		//stdout.printf(@"$deltat\n");
+		var deltat = 0.01;
+
+		i++;
+
+		//stdout.printf(@"$i $gx $gy $gz\n");
+
+		/* auxiliary variables to avoid reapeated calcualtions */
+		var halfSEq_1 = 0.5 * SEq_1;
+		var halfSEq_2 = 0.5 * SEq_2;
+		var halfSEq_3 = 0.5 * SEq_3;
+		var halfSEq_4 = 0.5 * SEq_4;
+		var twoSEq_1  = 2.0 * SEq_1;
+		var twoSEq_2  = 2.0 * SEq_2;
+		var twoSEq_3  = 2.0 * SEq_3;
+
+		/* compute the quaternion rate measured by gyroscopes */
+		var SEqDot_omega_1 = -halfSEq_2 * gx - halfSEq_3 * gy - halfSEq_4 * gz;
+		var SEqDot_omega_2 =  halfSEq_1 * gx + halfSEq_3 * gz - halfSEq_4 * gy;
+		var SEqDot_omega_3 =  halfSEq_1 * gy - halfSEq_2 * gz + halfSEq_4 * gx;
+		var SEqDot_omega_4 =  halfSEq_1 * gz + halfSEq_2 * gy - halfSEq_3 * gx;
+
+		/* normalise the accelerometer measurement */
+		var norm = Math.sqrt(ax * ax + ay * ay + az * az);
+		ax /= norm;
+		ay /= norm;
+		az /= norm;
+
+		/* compute the objective function and Jacobian */
+		var f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - ax;
+		var f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - ay;
+		var f_3 = 1.0 - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - az;
+
+		/* J_11 negated in matrix multiplication */
+		var J_11or24 = twoSEq_3;
+		var J_12or23 = 2 * SEq_4;
+
+		/* J_12 negated in matrix multiplication */
+		var J_13or22 = twoSEq_1;
+		var J_14or21 = twoSEq_2;
+
+		/* negated in matrix multiplication */
+		var J_32 = 2 * J_14or21;
+
+		/* negated in matrix multiplication */
+		var J_33 = 2 * J_11or24;
+
+		/* compute the gradient (matrix multiplication) */
+		var nablaf_1 = J_14or21 * f_2 - J_11or24 * f_1;
+		var nablaf_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
+		var nablaf_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
+		var nablaf_4 = J_14or21 * f_1 + J_11or24 * f_2;
+
+		/* normalise the gradient */
+		norm = Math.sqrt(nablaf_1 * nablaf_1 + nablaf_2 * nablaf_2 + nablaf_3 * nablaf_3 + nablaf_4 * nablaf_4);
+		nablaf_1 /= norm;
+		nablaf_2 /= norm;
+		nablaf_3 /= norm;
+		nablaf_4 /= norm;
+
+		/* compute then integrate the estimated quaternion rate */
+		SEq_1 += (SEqDot_omega_1 - (beta * nablaf_1)) * deltat;
+		SEq_2 += (SEqDot_omega_2 - (beta * nablaf_2)) * deltat;
+		SEq_3 += (SEqDot_omega_3 - (beta * nablaf_3)) * deltat;
+		SEq_4 += (SEqDot_omega_4 - (beta * nablaf_4)) * deltat;
 
 		/* normalise quaternion */
-		norm = Math.sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
-		q0 = q0 / norm;
-		q1 = q1 / norm;
-		q2 = q2 / norm;
-		q3 = q3 / norm;
-
-		/* save the adjusted gyroscope measurements */
-		correctedRateVector[AXIS.X] = gx;
-		correctedRateVector[AXIS.Y] = gy;
-		correctedRateVector[AXIS.Z] = gz;
-
-		/* calculate euler angles */
-		kinematicsAngle[AXIS.X] = Math.atan2(2 * (q0*q1 + q2*q3), 1 - 2 *(q1*q1 + q2*q2));
-		kinematicsAngle[AXIS.Y] = Math.asin(2 * (q0*q2 - q1*q3));
-		kinematicsAngle[AXIS.Z] = Math.atan2(2 * (q0*q3 + q1*q2), 1 - 2 *(q2*q2 + q3*q3));
-
-		log("Kinematics", LogLevelFlags.LEVEL_INFO, @"angles: $(kinematicsAngle[AXIS.X]), $(kinematicsAngle[AXIS.Y]), $(kinematicsAngle[AXIS.Z])");
+		norm = Math.sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
+		SEq_1 /= norm;
+		SEq_2 /= norm;
+		SEq_3 /= norm;
+		SEq_4 /= norm;
+
+		if(firstUpdate) {
+			/* store orientation of auxiliary frame. */
+			AEq_1 = SEq_1;
+			AEq_2 = SEq_2;
+			AEq_3 = SEq_3;
+			AEq_4 = SEq_4;
+			firstUpdate = false;
+		}
+
+		compute_euler();
+	}
+
+	private void compute_euler() {
+		/* compute the quaternion conjugate */
+		var ESq_1 =  SEq_1;
+		var ESq_2 = -SEq_2;
+		var ESq_3 = -SEq_3;
+		var ESq_4 = -SEq_4;
+
+		/* compute the quaternion product */
+		var ASq_1 = ESq_1 * AEq_1 - ESq_2 * AEq_2 - ESq_3 * AEq_3 - ESq_4 * AEq_4;
+		var ASq_2 = ESq_1 * AEq_2 + ESq_2 * AEq_1 + ESq_3 * AEq_4 - ESq_4 * AEq_3;
+		var ASq_3 = ESq_1 * AEq_3 - ESq_2 * AEq_4 + ESq_3 * AEq_1 + ESq_4 * AEq_2;
+		var ASq_4 = ESq_1 * AEq_4 + ESq_2 * AEq_3 - ESq_3 * AEq_2 + ESq_4 * AEq_1;
+
+		/* compute the Euler angles from the quaternion */
+		var phi   = Math.atan2(2 * ASq_3 * ASq_4 - 2 * ASq_1 * ASq_2, 2 * ASq_1 * ASq_1 + 2 * ASq_4 * ASq_4 - 1);
+		var theta = Math.asin(2 * ASq_2 * ASq_4 - 2 * ASq_1 * ASq_3);
+		var psi   = Math.atan2(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_4, 2 * ASq_1 * ASq_1 + 2 * ASq_2 * ASq_2 - 1);
+
+		result[AXIS.X] = phi;   /* roll */
+		result[AXIS.Y] = theta; /* pitch */
+		result[AXIS.Z] = psi;   /* yaw */
+
+		stdout.printf(@"$i $(degree(result[AXIS.X]))\t$(degree(result[AXIS.Y]))\t$(degree(result[AXIS.Z]))\n");
+	}
+
+	public double[] get_data() {
+		return result;
+	}
+
+	public void reset() {
+		firstUpdate = true;
+
+		/* Quaternion orientation of earth frame relative to auxiliary frame. */
+		AEq_1 = 1;
+		AEq_2 = 0;
+		AEq_3 = 0;
+		AEq_4 = 0;
+
+		/* Estimated orientation quaternion elements with initial conditions. */
+		SEq_1 = 1;
+		SEq_2 = 0;
+		SEq_3 = 0;
+		SEq_4 = 0;
 	}
 }