From 05ede46c5f16b9a30c5c6cc86ddcd494d7517fbc Mon Sep 17 00:00:00 2001 From: Sebastian Reichel Date: Mon, 29 Oct 2012 16:37:28 +0100 Subject: use kinematics module from mbed.org --- ctrl/kinematics.vala | 309 ++++++++++++++++++++++++++++----------------------- 1 file 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 - * Copyright 2012, Ted Carancho +/* Copyright 2010, ARM Limited + * Original C++ Code written by Aaron Berk + * Ported to Vala by Sebastian Reichel * - * 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 . + * 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; } } -- cgit v1.2.3