IMU/IMU.java at master · xinlongj/IMU · GitHub

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
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
package com.example.imu;

import android.util.Log;

public class IMU {
	public IMU(){
		Log.w(TAG, "IMU()");
	}

	// Function to compute one filter iteration
	public Quaternion filterUpdate(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z, double m_x, double m_y, double m_z)
	{
		// local system variables
		double norm; // vector norm
		double SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4; // quaternion rate from gyroscopes elements
		double f_1, f_2, f_3, f_4, f_5, f_6; // objective function elements
		double J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33, // objective function Jacobian(雅克比) elements
			J_41, J_42, J_43, J_44, J_51, J_52, J_53, J_54, J_61, J_62, J_63, J_64; //
		double SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error
		double w_err_x, w_err_y, w_err_z; // estimated direction of the gyroscope error (angular)
		double h_x, h_y, h_z; // computed flux in the earth frame
		// axulirary variables to avoid reapeated calcualtions
		double halfSEq_1 = 0.5f * SEq_1;
		double halfSEq_2 = 0.5f * SEq_2;
		double halfSEq_3 = 0.5f * SEq_3;
		double halfSEq_4 = 0.5f * SEq_4;

		double twoSEq_1 = 2.0f * SEq_1;
		double twoSEq_2 = 2.0f * SEq_2;
		double twoSEq_3 = 2.0f * SEq_3;
		double twoSEq_4 = 2.0f * SEq_4;

		double twob_x = 2.0f * b_x;
		double twob_z = 2.0f * b_z;

		double twob_xSEq_1 = 2.0f * b_x * SEq_1;
		double twob_xSEq_2 = 2.0f * b_x * SEq_2;
		double twob_xSEq_3 = 2.0f * b_x * SEq_3;
		double twob_xSEq_4 = 2.0f * b_x * SEq_4;

		double twob_zSEq_1 = 2.0f * b_z * SEq_1;
		double twob_zSEq_2 = 2.0f * b_z * SEq_2;
		double twob_zSEq_3 = 2.0f * b_z * SEq_3;
		double twob_zSEq_4 = 2.0f * b_z * SEq_4;

		double SEq_1SEq_2;
		double SEq_1SEq_3 = SEq_1 * SEq_3;
		double SEq_1SEq_4;
		double SEq_2SEq_3;
		double SEq_2SEq_4 = SEq_2 * SEq_4;
		double SEq_3SEq_4;

		double twom_x = 2.0f * m_x;
		double twom_y = 2.0f * m_y;
		double twom_z = 2.0f * m_z;

		// calculate the delta Time
		curTimeInMs = System.currentTimeMillis();
		if(curTimeInMs - lastTimeInMs < 100){
			deltat = (curTimeInMs - lastTimeInMs) / 1000;
			lastTimeInMs = curTimeInMs;
		}

		// normalise the accelerometer measurement
		norm = Math.sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
		a_x /= norm;
		a_y /= norm;
		a_z /= norm;
		// normalise the magnetometer measurement
		norm = Math.sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
		m_x /= norm;
		m_y /= norm;
		m_z /= norm;

		// compute the objective function and Jacobian
		f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;
		f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;
		f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;

		f_4 = twob_x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twob_z * (SEq_2SEq_4 - SEq_1SEq_3) - m_x;
		f_5 = twob_x * (SEq_2 * SEq_3 - SEq_1 * SEq_4) + twob_z * (SEq_1 * SEq_2 + SEq_3 * SEq_4) - m_y;
		f_6 = twob_x * (SEq_1SEq_3 + SEq_2SEq_4) + twob_z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3) - m_z;

		J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication
		J_12or23 = 2.0f * SEq_4;
		J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication
		J_14or21 = twoSEq_2;
		J_32 = 2.0f * J_14or21; // negated in matrix multiplication
		J_33 = 2.0f * J_11or24; // negated in matrix multiplication
		J_41 = twob_zSEq_3; // negated in matrix multiplication
		J_42 = twob_zSEq_4;
		J_43 = 2.0f * twob_xSEq_3 + twob_zSEq_1; // negated in matrix multiplication
		J_44 = 2.0f * twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication
		J_51 = twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication
		J_52 = twob_xSEq_3 + twob_zSEq_1;
		J_53 = twob_xSEq_2 + twob_zSEq_4;
		J_54 = twob_xSEq_1 - twob_zSEq_3; // negated in matrix multiplication
		J_61 = twob_xSEq_3;
		J_62 = twob_xSEq_4 - 2.0f * twob_zSEq_2;
		J_63 = twob_xSEq_1 - 2.0f * twob_zSEq_3;
		J_64 = twob_xSEq_2;

		// compute the gradient (matrix multiplication) 梯度
		SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1 - J_41 * f_4     - J_51 * f_5 + J_61 * f_6;
		SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3     + J_42 * f_4 + J_52 * f_5 + J_62 * f_6;
		SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3     - J_13or22 * f_1 - J_43 * f_4 + J_53 * f_5 + J_63 * f_6;
		SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2 - J_44 * f_4     - J_54 * f_5 + J_64 * f_6;

		// normalise the gradient to estimate direction of the gyroscope error
		norm = Math.sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
		SEqHatDot_1 = SEqHatDot_1 / norm;
		SEqHatDot_2 = SEqHatDot_2 / norm;
		SEqHatDot_3 = SEqHatDot_3 / norm;
		SEqHatDot_4 = SEqHatDot_4 / norm;

		// compute angular estimated direction of the gyroscope error
		w_err_x = twoSEq_1 * SEqHatDot_2 - twoSEq_2 * SEqHatDot_1 - twoSEq_3 * SEqHatDot_4 + twoSEq_4 * SEqHatDot_3;
		w_err_y = twoSEq_1 * SEqHatDot_3 + twoSEq_2 * SEqHatDot_4 - twoSEq_3 * SEqHatDot_1 - twoSEq_4 * SEqHatDot_2;
		w_err_z = twoSEq_1 * SEqHatDot_4 - twoSEq_2 * SEqHatDot_3 + twoSEq_3 * SEqHatDot_2 - twoSEq_4 * SEqHatDot_1;

		// compute and remove the gyroscope baises
		w_bx += w_err_x * deltat * zeta;
		w_by += w_err_y * deltat * zeta;
		w_bz += w_err_z * deltat * zeta;
		w_x -= w_bx;
		w_y -= w_by;
		w_z -= w_bz;

		// compute the quaternion rate measured by gyroscopes
		SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;
		SEqDot_omega_2 =  halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;
		SEqDot_omega_3 =  halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;
		SEqDot_omega_4 =  halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;

		// compute then integrate the estimated quaternion rate
		SEq_1 += (SEqDot_omega_1 - (beta * SEqHatDot_1)) * deltat;
		SEq_2 += (SEqDot_omega_2 - (beta * SEqHatDot_2)) * deltat;
		SEq_3 += (SEqDot_omega_3 - (beta * SEqHatDot_3)) * deltat;
		SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat;

		// normalise quaternion
		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;

		// compute flux in the earth frame
		SEq_1SEq_2 = SEq_1 * SEq_2; // recompute axulirary variables
		SEq_1SEq_3 = SEq_1 * SEq_3;
		SEq_1SEq_4 = SEq_1 * SEq_4;
		SEq_2SEq_3 = SEq_2 * SEq_3;
		SEq_2SEq_4 = SEq_2 * SEq_4;
		SEq_3SEq_4 = SEq_3 * SEq_4;

		h_x = twom_x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twom_y * (SEq_2SEq_3 - SEq_1SEq_4) + twom_z * (SEq_2SEq_4 + SEq_1SEq_3);
		h_y = twom_x * (SEq_2SEq_3 + SEq_1SEq_4) + twom_y * (0.5f - SEq_2 * SEq_2 - SEq_4 * SEq_4) + twom_z * (SEq_3SEq_4 - SEq_1SEq_2);
		h_z = twom_x * (SEq_2SEq_4 - SEq_1SEq_3) + twom_y * (SEq_3SEq_4 + SEq_1SEq_2) + twom_z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3);

		// normalise the flux vector to have only components in the x and z
		b_x = Math.sqrt((h_x * h_x) + (h_y * h_y));
		b_z = h_z;
		Quaternion quaternion = new Quaternion(SEq_1, SEq_2, SEq_3, SEq_4);
		return quaternion;
	}

	public void reset(){
		SEq_1 = 1;
		SEq_2 = 0;
		SEq_3 = 0;
		SEq_4 = 0;

		b_x = 1;
		b_z = 0;

		w_bx = 0;
		w_by = 0;
		w_bz = 0;
	}

	private static final String TAG = "IMU";
	private static double lastTimeInMs = 0f;
	private static double curTimeInMs = 0f;
	private static  double deltat = 0.05f; // 采样频率 sampling period in seconds (shown as 1 ms)
	private static final double gyroMeasError  = 3.14159265358979 * (5.0f / 180.0f); // 陀螺仪的角速度测量误差	gyroscope measurement error in rad/s (shown as 5 deg/s)
	private static final double gyroMeasDrift = 3.14159265358979 * (0.2f / 180.0f); // 陀螺仪的角加速度测量误差	gyroscope measurement error in rad/s/s (shown as 0.2f deg/s/s)
	private static final double beta = Math.sqrt(3.0f / 4.0f) * gyroMeasError; // compute beta
	private static final double zeta = Math.sqrt(3.0f / 4.0f) * gyroMeasDrift; // compute zeta
	// Global system variables
	//private static double a_x, a_y, a_z; // accelerometer measurements
	//private static double w_x, w_y, w_z; // gyroscope measurements in rad/s
	//private static double m_x, m_y, m_z; // magnetometer measurements
	private static double SEq_1 = 1, SEq_2 = 0, SEq_3 = 0, SEq_4 = 0; // estimated orientation quaternion(四元数) elements with initial conditions
	private static double b_x = 1, b_z = 0; // reference direction of flux in earth frame
	private static double w_bx = 0, w_by = 0, w_bz = 0; // estimate gyroscope biases error
}