Skip to content

Commit ea41290

Browse files
RCmagsRCmags
committed
simpler accIntegral
1 parent ed15754 commit ea41290

File tree

7 files changed

+84
-65
lines changed

7 files changed

+84
-65
lines changed

‎README.md‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3,15 +3,17 @@ This library fuses the outputs of an inertial measurement unit (IMU) and stores
33

44
$\ \overrightarrow{a_{rel}} = \overrightarrow{a_{local}} - (0,0,1) $
55

6-
$\ K_{\sigma} = {\alpha}/(1 + \frac{ {\sigma}^2 }{ {\sigma}_{acc}^2 } ) $
6+
$\ K_{\sigma} = 1/(1 + \frac{ {\sigma}^2 }{ {\sigma}_{acc}^2 } ) $
77

88
$\ {\sigma}^2 = | \overrightarrow{a_{rel}} |^2 + K_{\sigma}{\sigma}^2 $
99

1010
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:
1111

1212
$\ E_{k} = \theta_{accel} - \theta_{k-1} $
1313

14-
$\ \theta_{k} = \theta_{k-1} + \dot{\theta}{\Delta t} + K_{\sigma}E_{k} $
14+
$\ K_{\theta} = {\alpha} K_{\sigma} {\Delta t} $
15+
16+
$\ \theta_{k} = \theta_{k-1} + \dot{\theta}{\Delta t} + K_{\theta}E_{k} $
1517

1618
As the filter uses a quaternion to encode rotations, it's easy to perform coordinate transformations. The library has functions to:
1719
- Transfor a vector to the local or global frame.

‎examples/velocity/velocity.ino‎

Lines changed: 34 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -4,11 +4,11 @@
44

55
/* NOTE:
66
1. The accelerometer MUST be calibrated to integrate its output. Adjust the
7-
AX, AY, AND AZ offsets until the sensor reads (0,0,0) at rest.
7+
AX, AY, AND AZ offsets until the sensor reads (0,0,GRAVITY) at rest.
88
99
2. REQUIRED UNITS:
1010
I. The gyroscope must output: radians/second
11-
II. The accelerometer output: g-force [1 g-force ~ 9.81 meter/second^2]
11+
II. The accelerometer output: length/second^2 [The length unit is arbitrary. Can be meter, feet, etc]
1212
*/
1313

1414
#include <basicMPU6050.h> // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
@@ -35,50 +35,59 @@ basicMPU6050<LP_FILTER, GYRO_SENS, ACCEL_SENS, ADDRESS_A0,
3535

3636
// =========== Settings ===========
3737
accIntegral fusion;
38-
// Unit
39-
constexpr float GRAVITY = 9.81e3; // mm/s^2 Magnitude of gravity at rest. Determines units of velocity.
40-
constexpr float SD_ACC = 1000; // mm/s^2 Standard deviation of acceleration. Deviations from zero are suppressed.
41-
constexpr float SD_VEL = 200; // mm/s Standard deviation of velocity. Deviations from target value are suppressed.
42-
constexpr float ALPHA = 0.5; // Gain of heading estimate - See example "output" [optional parameter]
38+
39+
// Filter coefficients // Unit
40+
constexpr float GRAVITY = 9.81e3; // mm/s^2 Magnitude of gravity at rest. Determines units of velocity. [UNITS MUST MATCH ACCELERATION]
41+
constexpr float SD_ACC = 1000; // mm/s^2 Standard deviation of acceleration. Deviations from zero are suppressed.
42+
constexpr float SD_VEL = 200; // mm/s Standard deviation of velocity. Deviations from target value are suppressed.
43+
constexpr float ALPHA = 0.5; // Gain of heading update - See example "output" for more information.
44+
45+
// Sensor scaling
46+
constexpr float TO_LENGTH_PER_SECOND_SQ = GRAVITY; // Constant to convert acceleration measurements to: length/second^2
47+
constexpr float TO_RAD_PER_SECOND = 1.0; // Constant to convery gyroscope measurements to: radians/second
4348

4449
void setup() {
4550
Serial.begin(38400);
51+
delay(1000);
4652

4753
// calibrate IMU sensor
4854
imu.setup();
4955
imu.setBias();
5056

5157
// initialize sensor fusion
52-
fusion.setup( imu.ax(), imu.ay(), imu.az() ); // set initial heading
53-
54-
/* Use this function to zero velocity estimate */
55-
//fusion.reset();
58+
//fusion.setup( imu.ax(), imu.ay(), imu.az() ); // ALWAYS set initial heading with acceleration
59+
fusion.setup();
60+
61+
//fusion.reset(); /* Use this function to zero velocity estimate */
5662
}
5763

5864
void loop() {
59-
/* NOTE: The heading must be updated along with the velocity estimate for accurate results.
60-
Use the following steps to ensure proper integration */
61-
62-
// 1. measure state
63-
6465
imu.updateBias();
66+
67+
// Measure state:
6568
vec3_t accel = { imu.ax(), imu.ay(), imu.az() };
6669
vec3_t gyro = { imu.gx(), imu.gy(), imu.gz() };
6770

68-
// 2. update heading and velocity estimate
69-
70-
vec3_t vel_t = {0,0,0}; // Known measured velocity (target state)
71-
72-
/* ALPHA is optional - can be excluded */
71+
// Scale measurements
72+
accel *= TO_LENGTH_PER_SECOND_SQ;
73+
gyro *= TO_RAD_PER_SECOND;
74+
75+
// Update heading and velocity estimate:
76+
77+
// known measured velocity (target state). Estimate will be forced towards this vector
78+
vec3_t vel_t = {0,0,0};
79+
80+
/* note: all coefficients are optional and have default values */
7381
fusion.update( gyro, accel, vel_t, SD_ACC, SD_VEL, GRAVITY, ALPHA );
74-
vec3_t vel = fusion.getVel();
82+
83+
// obtain velocity estimate
84+
vec3_t vel = fusion.getVel();
7585

76-
// Display velocity components [view with serial plotter]
77-
86+
// Display velocity components: [view with serial plotter]
7887
Serial.print( vel.x, 2 );
7988
Serial.print( " " );
8089
Serial.print( vel.y, 2 );
8190
Serial.print( " " );
82-
Serial.print( vel.z, 2 );
91+
Serial.print( vel.z, 2 );
8392
Serial.println();
8493
}

‎library.properties‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
name=imuFilter
2-
version=1.6.0
2+
version=1.6.1
33
author=RCmags <[email protected]>
44
maintainer=RCmags <[email protected]>
55
sentence=Sensor fusion for an IMU to obtain heading and velocity.

‎src/accIntegral.cpp‎

Lines changed: 5 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ void accIntegral::update( vec3_t angvel,
2424
vec3_t accel,
2525
vec3_t vel_t,
2626
const float SD_ACC,
27-
const float SD_VEL,
27+
const float SD_VEL,
2828
const float GRAVITY,
2929
const float ALPHA ) {
3030
// 0. Constants:
@@ -33,11 +33,9 @@ void accIntegral::update( vec3_t angvel,
3333
const float VAR_COMB = VAR_ACC*VAR_VEL;
3434

3535
// 1. Remove acceleration bias:
36-
imuFilter::update( angvel, accel, ALPHA, SD_ACC );
36+
imuFilter::update( angvel, accel, ALPHA, SD_ACC, GRAVITY );
3737
quat_t qt = imuFilter::getQuat();
38-
39-
accel *= GRAVITY; // convert to given units
40-
38+
4139
// Remove centrifugal
4240
vec3_t vel_local = qt.rotate(vel, LOCAL_FRAME);
4341
accel -= vel_local.cross(angvel);
@@ -78,11 +76,11 @@ void accIntegral::update( float gx, float gy, float gz,
7876
float ax, float ay, float az,
7977
float vx, float vy, float vz,
8078
const float SD_ACC,
81-
const float SD_VEL,
79+
const float SD_VEL,
8280
const float GRAVITY,
8381
const float ALPHA ) {
8482
vec3_t angvel = {gx, gy, gz};
8583
vec3_t accel = {ax, ay, az};
8684
vec3_t vel_t = {vx, vy, vz};
87-
return update( angvel, accel, vel_t, ALPHA, SD_ACC, SD_VEL, GRAVITY );
85+
return update( angvel, accel, vel_t, SD_ACC, SD_VEL, GRAVITY, ALPHA );
8886
}

‎src/accIntegral.h‎

Lines changed: 11 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,10 @@
33
#ifndef accIntegral_h
44
#define accIntegral_h
55

6+
//------------------ Coefficients --------------------
7+
8+
#define DEFAULT_SD_VEL 0.1 // Default standard deviation in velocity. [g-force * second]
9+
610
//---------------- Class definition ------------------
711

812
class accIntegral: public imuFilter {
@@ -22,17 +26,17 @@ class accIntegral: public imuFilter {
2226
vec3_t getVel();
2327

2428
void update( vec3_t, vec3_t, vec3_t,
25-
const float,
26-
const float,
27-
const float,
29+
const float=DEFAULT_SD_ACC,
30+
const float=DEFAULT_SD_VEL,
31+
const float=DEFAULT_GRAVITY,
2832
const float=DEFAULT_GAIN );
2933

3034
void update( float, float, float,
3135
float, float, float,
32-
float, float, float,
33-
const float,
34-
const float,
35-
const float,
36+
float, float, float,
37+
const float=DEFAULT_SD_ACC,
38+
const float=DEFAULT_SD_VEL,
39+
const float=DEFAULT_GRAVITY,
3640
const float=DEFAULT_GAIN );
3741
};
3842

‎src/imuFilter.cpp‎

Lines changed: 19 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,10 @@
22

33
//----------------- Initialization -------------------
44

5+
float imuFilter::timeStep() {
6+
return dt;
7+
}
8+
59
void imuFilter::updateTimer() {
610
uint32_t time_now = micros();
711
uint32_t change_time = time_now - last_time;
@@ -15,10 +19,10 @@ void imuFilter::setup() {
1519
last_time = micros();
1620
}
1721

18-
void imuFilter::setup( float ax, float ay, float az ) {
22+
void imuFilter::setup( vec3_t accel ) {
1923
setup();
2024
// set quaternion as vertical vector
21-
vec3_t v = vec3_t(ax, ay, az).norm(); // gravity vector
25+
vec3_t v = accel.norm(); // gravity vector
2226

2327
float norm = v.x*v.x + v.y*v.y;
2428
float cosine = v.z/sqrt( norm + v.z*v.z ) * 0.5; // vertical angle
@@ -28,8 +32,8 @@ void imuFilter::setup( float ax, float ay, float az ) {
2832
q.v = vec3_t(v.y*norm, -v.x*norm, 0);
2933
}
3034

31-
void imuFilter::setup( vec3_t accel ) {
32-
setup( accel.x, accel.y, accel.z );
35+
void imuFilter::setup( float ax, float ay, float az ) {
36+
setup( vec3_t(ax, ay, az) );
3337
}
3438

3539
//---------------- Heading estimate ------------------
@@ -53,29 +57,31 @@ void imuFilter::update( float gx, float gy, float gz ) {
5357

5458
void imuFilter::update( float gx, float gy, float gz,
5559
float ax, float ay, float az,
56-
const float ALPHA /*=DEFAULT_GAIN*/,
57-
const float SD_ACC /*=DEFAULT_SD*/ ) {
60+
const float ALPHA,
61+
const float SD_ACC,
62+
const float GRAVITY ) {
5863
// Update Timer
5964
updateTimer();
6065

6166
// check global acceleration:
6267
vec3_t accel = {ax, ay, az};
63-
vec3_t acrel = q.rotate(accel, GLOBAL_FRAME) - vec3_t(0,0,1);
68+
vec3_t acrel = q.rotate(accel, GLOBAL_FRAME);
69+
acrel.z -= GRAVITY;
6470

6571
// kalmal filter:
6672
const float INV_VAR = 1.0/( SD_ACC * SD_ACC );
6773
float error = acrel.dot(acrel); // deviation from vertical
6874

69-
float gain = ALPHA * dt;
70-
gain = gain/(1 + var*INV_VAR); // kalman gain
75+
float gain = 1.0/(1 + var*INV_VAR); // kalman gain
7176
var = error + var*gain; // variance of error
72-
77+
7378
// error about vertical
7479
vec3_t vz = q.axisZ(LOCAL_FRAME);
7580
vec3_t ve = accel.norm();
7681
ve = ve.cross(vz);
7782

7883
// Rotation increment
84+
gain *= ALPHA * dt;
7985
vec3_t da = vec3_t(gx, gy, gz)*dt + ve*gain;
8086
quat_t dq; dq.setRotation(da, SMALL_ANGLE);
8187

@@ -90,8 +96,9 @@ void imuFilter::update( vec3_t gyro ) {
9096
}
9197

9298
void imuFilter::update( vec3_t gyro, vec3_t accel,
93-
const float ALPHA /*=DEFAULT_GAIN*/,
94-
const float SD_ACC /*=DEFAULT_SD*/ ) {
99+
const float ALPHA,
100+
const float SD_ACC,
101+
const float GRAVITY ) {
95102
update( gyro.x , gyro.y , gyro.z,
96103
accel.x, accel.y, accel.z,
97104
ALPHA, SD_ACC );
@@ -134,10 +141,6 @@ vec3_t imuFilter::projectVector( vec3_t vec, const bool TO_WORLD ) {
134141
return q.rotate(vec, TO_WORLD);
135142
}
136143

137-
float imuFilter::timeStep() {
138-
return dt;
139-
}
140-
141144
//------------------ Euler Angles -------------------
142145

143146
float imuFilter::roll() { // x-axis

‎src/imuFilter.h‎

Lines changed: 10 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -8,17 +8,18 @@
88

99
#define INV_Q_FACTOR 2 // Filter damping. A smaller value leads to faster response but more oscillations.
1010
#define DEFAULT_GAIN 0.5 // Default filter gain.
11-
#define DEFAULT_SD 0.2 // Default standard deviation in acceleration. [g-force]
11+
#define DEFAULT_SD_ACC 0.2 // Default standard deviation in acceleration. [g-force]
12+
#define DEFAULT_GRAVITY 1.0 // Default acceleration due to gravity. [g-force]
1213

1314
//---------------- Class definition ------------------
1415

1516
class imuFilter {
1617
private:
17-
quat_t q;
18-
float var;
18+
quat_t q = {1,0,0,0};
19+
float var = 0;
20+
uint32_t last_time = 0;
1921
float dt;
20-
uint32_t last_time;
21-
22+
2223
void updateTimer();
2324

2425
public:
@@ -33,12 +34,14 @@ class imuFilter {
3334
void update( float, float, float,
3435
float, float, float,
3536
const float=DEFAULT_GAIN,
36-
const float=DEFAULT_SD );
37+
const float=DEFAULT_SD_ACC,
38+
const float=DEFAULT_GRAVITY );
3739

3840
void update( vec3_t );
3941
void update( vec3_t, vec3_t,
4042
const float=DEFAULT_GAIN,
41-
const float=DEFAULT_SD );
43+
const float=DEFAULT_SD_ACC,
44+
const float=DEFAULT_GRAVITY );
4245

4346
void rotateHeading( float, const bool );
4447

0 commit comments

Comments
 (0)