Skip to content

Commit 46010ec

Browse files
RCmagsRCmags
committed
upgrade to v1.4.0
1 parent 9cd86c4 commit 46010ec

File tree

9 files changed

+435
-606
lines changed

9 files changed

+435
-606
lines changed

‎README.md‎

Lines changed: 20 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1,23 +1,30 @@
11
# imuFilter
2+
This library fuses the outputs of an inertial measurement unit (IMU) and stores the heading as a quaternion. It uses a _kalman-like_ filter to check the acceleration and see if it lies within a deviation from (0,0,1)g. If the acceleration is within this band, it will strongly correct the orientation. However, if the acceleration lies outside of this band, it will barely affect the orientation. To this end, the deviation from vertical is used to update the variance and kalman gain:
23

3-
This library contains a sensor fusion algorithm to combine the outputs of a 6-axis inertial measurement unit (IMU). It's based on a modified version of the Mahony filter that replaces the PI controller with something akin to a 2nd order low pass filter. The proportional term was removed and the integral term has been forced to decay in order to damp the system. The correction steps of each filter are shown below:
4+
$\ \overrightarrow{a_{rel}} = \overrightarrow{a_{local}} - (0,0,1) $
45

5-
- Mahony:
6-
_integral += error.dt
7-
dtheta = theta_dot.dt + kp.error + ki.integral_
6+
$\ K_{\sigma} = {\alpha}/(1 + \frac{ {\sigma}^2 }{ {\sigma}_{acc}^2 } ) $
87

9-
- Modified Mahony:
10-
_integral += error.kp - integral.kc
11-
dtheta = theta_dot.dt + integral_
8+
$\ {\sigma}^2 = | \overrightarrow{a_{rel}} |^2 + K_{\sigma}{\sigma}^2 $
129

13-
The behavior of the modified filter is analogous to spring-mass system. Kp (stiffness) and Kc (damping) are related by the damping ratio Q which is held constant. This allows the behavior of the filter to be controlled via a single parameter.
10+
The kalman gain then scaled by a delay parameter and used to correct the attitude. This scaling allows the filter to act like a 1rst-order low pass filter that smoothens the acceleration at the cost of slower response:
1411

15-
The filter uses a quaternion to encode rotations. This makes it easy to perform coordinate transformations. These include:
16-
- Transfor a vector from the local frame to the global (and vice versa)
17-
- Get unit vectors of the X, Y and Z axes in the local or global frame.
12+
$\ E_{k} = \theta_{accel} - \theta_{k-1} $
1813

19-
Since a 6-axis IMU has no absolute reference for heading there is a function to rotate the orientation estimate about the yaw axis. Basic vector operations have been included to easily implement a heading correction algorithm should one have an additional sensor (such a magnetometer or some other absolute heading sensor).
14+
$\ \theta_{k} = \theta_{k-1} + \dot{\theta}{\Delta t} + K_{\sigma}E_{k} $
2015

21-
For more information on the Mahony filter see these links:
16+
As the filter uses a quaternion to encode rotations, it's easy to perform coordinate transformations. The library has functions to:
17+
- Transfor a vector to the local or global frame.
18+
- Get the unit vectors of the X, Y and Z axes in the local or global frame.
19+
20+
Moreover, since a 6-axis IMU (gyro-accelerometer) cannot measure an absolute heading, a function is included to rotate the orientation about the vertical (yaw) axis. One can use vector operations to correct the heading with an additional sensor like a magnetometer.
21+
22+
# Dependecies
23+
This library depends on the [vector_datatype](https://github.com/RCmags/vector_datatype) library.
24+
25+
# References
26+
See these links for more information on the Mahony filter:
27+
- [Nonlinear Complementary Filters on the Special
28+
Orthogonal Group](https://hal.archives-ouvertes.fr/hal-00488376/document) (original paper)
2229
- [IMU Data Fusing: Complementary, Kalman, and Mahony Filter](http://www.olliw.eu/2013/imu-data-fusing/#chapter23)
2330
- [Mahony Filter](https://nitinjsanket.github.io/tutorials/attitudeest/mahony)

‎examples/heading/heading.ino‎

Lines changed: 38 additions & 80 deletions
Original file line numberDiff line numberDiff line change
@@ -1,80 +1,38 @@
1-
/*
2-
This sketch shows to perform vector products and rotate heading (yaw angle) of the estimated orientation.
3-
*/
4-
5-
#include <imuFilter.h>
6-
#include <basicMPU6050.h> // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
7-
8-
// Sensor fusion
9-
constexpr float GAIN = 0.1; // Fusion gain, value between 0 and 1 - Determines response of heading correction with respect to gravity.
10-
imuFilter <&GAIN> fusion;
11-
12-
// Imu sensor
13-
basicMPU6050<> imu;
14-
15-
// Display function:
16-
void printVector( float r[] ) {
17-
Serial.print( r[0], 2 );
18-
Serial.print( "," );
19-
Serial.print( r[1], 2 );
20-
Serial.print( "," );
21-
Serial.print( r[2], 2 );
22-
Serial.println();
23-
}
24-
25-
void setup() {
26-
// Initialize filter:
27-
fusion.setup( imu.ax(), imu.ay(), imu.az() );
28-
29-
// Calibrate imu
30-
imu.setup();
31-
imu.setBias();
32-
33-
Serial.begin(38400);
34-
35-
// Vector operations:
36-
float v1[] = { 3, 1, -1 }; // Input Vector
37-
float axis_y[3], axis_z[3];
38-
39-
fusion.getYaxis( true, axis_y ); // Vectors to operate on [global axes]
40-
fusion.getZaxis( true, axis_z );
41-
42-
fusion.crossProduct( v1, axis_y ); // Cross product: V = V cross R ; Output is stored in V
43-
float v2[] = { v1[0], v1[1], v1[2] }; // Store product
44-
45-
fusion.normalizeVector( v1 ); // Norm: V = V/|V| ; Output is stored in V
46-
47-
float dot = fusion.dotProduct( v1, axis_z ); // Dot product: Input order does not matter
48-
49-
// Rotate heading:
50-
#define SMALL_ANG false
51-
// Small angle approximation = true
52-
// Exact angle rotation = false
53-
fusion.rotateHeading( SMALL_ANG, dot );
54-
55-
// Display results:
56-
Serial.print( "y = " );
57-
printVector( axis_y );
58-
Serial.print( "v2 = " );
59-
printVector( v2 );
60-
Serial.print( "v1 = " );
61-
printVector( v1 );
62-
Serial.print( "dot = ");
63-
Serial.println( dot );
64-
65-
// Wait for output to be read
66-
delay(10000);
67-
}
68-
69-
void loop() {
70-
// Update filter:
71-
fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );
72-
73-
// Display angles:
74-
Serial.print( fusion.pitch() );
75-
Serial.print( " " );
76-
Serial.print( fusion.yaw() );
77-
Serial.print( " " );
78-
Serial.print( fusion.roll() );
79-
Serial.println();
80-
}
1+
/*
2+
This sketch shows to rotate the heading (yaw angle) of the estimated orientation.
3+
*/
4+
5+
#include <basicMPU6050.h> // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
6+
#include <imuFilter.h>
7+
8+
basicMPU6050<> imu;
9+
10+
imuFilter fusion;
11+
12+
void setup() {
13+
Serial.begin(38400);
14+
15+
// Calibrate imu
16+
imu.setup();
17+
imu.setBias();
18+
19+
// Initialize filter:
20+
fusion.setup( imu.ax(), imu.ay(), imu.az() );
21+
22+
// Rotate heading:
23+
float angle = 45 * DEG_TO_RAD; // angle in radians to rotate heading about z-axis
24+
fusion.rotateHeading( angle, LARGE_ANGLE ); // Can choose LARGE_ANGLE or SMALL_ANGLE approximation
25+
}
26+
27+
void loop() {
28+
// Update filter:
29+
fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );
30+
31+
// Display angles:
32+
Serial.print( fusion.pitch() );
33+
Serial.print( " " );
34+
Serial.print( fusion.yaw() );
35+
Serial.print( " " );
36+
Serial.print( fusion.roll() );
37+
Serial.println();
38+
}

‎examples/output/output.ino‎

Lines changed: 86 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -1,52 +1,86 @@
1-
/*
2-
This sketch shows to initialize the filter and update it with the imu output. It also shows how to get the euler angles of the estimated heading orientation.
3-
*/
4-
5-
#include <imuFilter.h>
6-
#include <basicMPU6050.h> // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
7-
8-
// Sensor fusion
9-
constexpr float GAIN = 0.1; // Fusion gain, value between 0 and 1 - Determines heading correction with respect to gravity vector.
10-
imuFilter <&GAIN> fusion;
11-
12-
// Imu sensor
13-
basicMPU6050<> imu;
14-
15-
// Enable sensor fusion [ 1 = yes; 0 = no]
16-
#define FUSION 1
17-
18-
void setup() {
19-
#if FUSION
20-
// Set quaternion with gravity vector
21-
fusion.setup( imu.ax(), imu.ay(), imu.az() );
22-
#else
23-
// Just use gyro
24-
fusion.setup();
25-
#endif
26-
27-
// Calibrate imu
28-
imu.setup();
29-
imu.setBias();
30-
31-
Serial.begin(38400);
32-
}
33-
34-
void loop() {
35-
// Update filter:
36-
37-
#if FUSION
38-
//Fuse gyro and accelerometer
39-
fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() );
40-
#else
41-
// Only use gyroscope
42-
fusion.update( imu.gx(), imu.gy(), imu.gz() );
43-
#endif
44-
45-
// Display angles:
46-
Serial.print( fusion.pitch() );
47-
Serial.print( " " );
48-
Serial.print( fusion.yaw() );
49-
Serial.print( " " );
50-
Serial.print( fusion.roll() );
51-
Serial.println();
52-
}
1+
/*
2+
This sketch shows to initialize the filter and update it with the imu output. It also shows how to get the euler angles of the estimated heading orientation.
3+
*/
4+
5+
#include <basicMPU6050.h> // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
6+
#include <imuFilter.h>
7+
8+
// Gyro settings:
9+
#define LP_FILTER 3 // Low pass filter. Value from 0 to 6
10+
#define GYRO_SENS 0 // Gyro sensitivity. Value from 0 to 3
11+
#define ACCEL_SENS 0 // Accelerometer sensitivity. Value from 0 to 3
12+
#define ADDRESS_A0 LOW // I2C address from state of A0 pin. A0 -> GND : ADDRESS_A0 = LOW
13+
// A0 -> 5v : ADDRESS_A0 = HIGH
14+
// Accelerometer offset:
15+
constexpr int AX_OFFSET = 552; // Use these values to calibrate the accelerometer. The sensor should output 1.0g if held level.
16+
constexpr int AY_OFFSET = -241; // These values are unlikely to be zero.
17+
constexpr int AZ_OFFSET = -3185;
18+
19+
// Output scale:
20+
constexpr float AX_SCALE = 1.00457; // Multiplier for accelerometer outputs. Use this to calibrate the sensor. If unknown set to 1.
21+
constexpr float AY_SCALE = 0.99170;
22+
constexpr float AZ_SCALE = 0.98317;
23+
24+
constexpr float GX_SCALE = 0.99764; // Multiplier to gyro outputs. Use this to calibrate the sensor. If unknown set to 1.
25+
constexpr float GY_SCALE = 1.0;
26+
constexpr float GZ_SCALE = 1.01037;
27+
28+
// Bias estimate:
29+
#define GYRO_BAND 35 // Standard deviation of the gyro signal. Gyro signals within this band (relative to the mean) are suppresed.
30+
#define BIAS_COUNT 5000 // Samples of the mean of the gyro signal. Larger values provide better calibration but delay suppression response.
31+
32+
//-- Set the template parameters:
33+
34+
basicMPU6050<LP_FILTER, GYRO_SENS, ACCEL_SENS, ADDRESS_A0,
35+
AX_OFFSET, AY_OFFSET, AZ_OFFSET,
36+
&AX_SCALE, &AY_SCALE, &AZ_SCALE,
37+
&GX_SCALE, &GY_SCALE, &GZ_SCALE,
38+
GYRO_BAND, BIAS_COUNT
39+
>imu;
40+
41+
// =========== Settings ===========
42+
imuFilter fusion;
43+
44+
#define GAIN 0.5 /* Fusion gain, value between 0 and 1 - Determines orientation correction with respect to gravity vector.
45+
If set to 1 the gyroscope is dissabled. If set to 0 the accelerometer is dissabled (equivant to gyro-only) */
46+
47+
#define SD_ACCEL 0.2 /* Standard deviation of acceleration. Accelerations relative to (0,0,1)g outside of this band are suppresed.
48+
Accelerations within this band are used to update the orientation. [Measured in g-force] */
49+
50+
#define FUSION true /* Enable sensor fusion. Setting to "true" enables gravity correction */
51+
52+
void setup() {
53+
Serial.begin(38400);
54+
55+
// Calibrate imu
56+
imu.setup();
57+
imu.setBias();
58+
59+
#if FUSION
60+
Set quaternion with gravity vector
61+
fusion.setup( imu.ax(), imu.ay(), imu.az() );
62+
#else
63+
Just use gyro
64+
fusion.setup();
65+
#endif
66+
}
67+
68+
void loop() {
69+
// Update filter:
70+
71+
#if FUSION
72+
/*NOTE: GAIN and SD_ACCEL are optional parameters */
73+
fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az(), GAIN, SD_ACCEL );
74+
#else
75+
// Only use gyroscope
76+
fusion.update( imu.gx(), imu.gy(), imu.gz() );
77+
#endif
78+
79+
// Display angles:
80+
Serial.print( fusion.pitch() );
81+
Serial.print( " " );
82+
Serial.print( fusion.yaw() );
83+
Serial.print( " " );
84+
Serial.print( fusion.roll() );
85+
Serial.println();
86+
}

0 commit comments

Comments
 (0)