1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
|
/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
* Copyright 2012, Ted Carancho
*
* Ported from AeroQuad
*
* 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.
*
* 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/>.
*/
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);
}
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;
/* 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])");
}
}
|
