Skip to content

Commit ed15754

Browse files
RCmagsRCmags
committed
timestep and update
1 parent e8ad992 commit ed15754

File tree

7 files changed

+79
-73
lines changed

7 files changed

+79
-73
lines changed

‎examples/output/output.ino‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -70,5 +70,10 @@ void loop() {
7070
Serial.print( fusion.yaw() );
7171
Serial.print( " " );
7272
Serial.print( fusion.roll() );
73+
74+
// timestep
75+
Serial.print( " " );
76+
Serial.print( fusion.timeStep(), 6 );
77+
7378
Serial.println();
7479
}

‎examples/velocity/velocity.ino‎

Lines changed: 17 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -2,8 +2,13 @@
22
This sketch shows to integrate acceleration to obtain an estimate of velocity
33
*/
44

5-
/* NOTE: The accelerometer MUST be calibrated to integrate its output. Adjust the
6-
AX, AY, AND AZ offsets until the sensor reads (0,0,0) at rest.
5+
/* NOTE:
6+
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.
8+
9+
2. REQUIRED UNITS:
10+
I. The gyroscope must output: radians/second
11+
II. The accelerometer output: g-force [1 g-force ~ 9.81 meter/second^2]
712
*/
813

914
#include <basicMPU6050.h> // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
@@ -32,8 +37,9 @@ basicMPU6050<LP_FILTER, GYRO_SENS, ACCEL_SENS, ADDRESS_A0,
3237
accIntegral fusion;
3338
// Unit
3439
constexpr float GRAVITY = 9.81e3; // mm/s^2 Magnitude of gravity at rest. Determines units of velocity.
35-
constexpr float SD_ACC = 200; // mm/s^2 Standard deviation of acceleration. Deviations from zero are suppressed.
40+
constexpr float SD_ACC = 1000; // mm/s^2 Standard deviation of acceleration. Deviations from zero are suppressed.
3641
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]
3743

3844
void setup() {
3945
Serial.begin(38400);
@@ -44,7 +50,9 @@ void setup() {
4450

4551
// initialize sensor fusion
4652
fusion.setup( imu.ax(), imu.ay(), imu.az() ); // set initial heading
47-
fusion.reset(); // zero velocity estimate
53+
54+
/* Use this function to zero velocity estimate */
55+
//fusion.reset();
4856
}
4957

5058
void loop() {
@@ -57,17 +65,13 @@ void loop() {
5765
vec3_t accel = { imu.ax(), imu.ay(), imu.az() };
5866
vec3_t gyro = { imu.gx(), imu.gy(), imu.gz() };
5967

60-
// 2. update heading
61-
62-
fusion.update( gyro, accel );
63-
64-
// 3. update velocity estimate
68+
// 2. update heading and velocity estimate
6569

6670
vec3_t vel_t = {0,0,0}; // Known measured velocity (target state)
67-
vec3_t vel; // placeholder variable
68-
69-
fusion.updateVel( gyro, accel, vel_t, SD_ACC, SD_VEL, GRAVITY );
70-
vel = fusion.getVel();
71+
72+
/* ALPHA is optional - can be excluded */
73+
fusion.update( gyro, accel, vel_t, SD_ACC, SD_VEL, GRAVITY, ALPHA );
74+
vec3_t vel = fusion.getVel();
7175

7276
// Display velocity components [view with serial plotter]
7377

‎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.5.0
2+
version=1.6.0
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: 22 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -11,38 +11,31 @@ void accIntegral::reset() {
1111
// scalars
1212
var_vel = 0;
1313
var_acc = 0;
14-
time_last = micros();
1514
}
1615

17-
//---------------------- Other -----------------------
16+
//---------------- Velocity estimate -----------------
1817

1918
vec3_t accIntegral::getVel() {
2019
return vel;
2120
}
2221

23-
float accIntegral::updateTimer() {
24-
uint32_t time_now = micros();
25-
uint32_t change_time = time_now - time_last;
26-
time_last = time_now;
27-
return float(change_time)*1e-6;
28-
}
29-
30-
//---------------- Velocity estimate -----------------
31-
3222
// update velocity with imu measurements
33-
void accIntegral::updateVel( vec3_t angvel,
34-
vec3_t accel,
35-
vec3_t vel_t,
36-
const float SD_ACC,
37-
const float SD_VEL,
38-
const float GRAVITY ) {
23+
void accIntegral::update( vec3_t angvel,
24+
vec3_t accel,
25+
vec3_t vel_t,
26+
const float SD_ACC,
27+
const float SD_VEL,
28+
const float GRAVITY,
29+
const float ALPHA ) {
3930
// 0. Constants:
4031
const float VAR_ACC = SD_ACC*SD_ACC;
4132
const float VAR_VEL = SD_VEL*SD_VEL;
4233
const float VAR_COMB = VAR_ACC*VAR_VEL;
4334

4435
// 1. Remove acceleration bias:
45-
quat_t qt = this->getQuat();
36+
imuFilter::update( angvel, accel, ALPHA, SD_ACC );
37+
quat_t qt = imuFilter::getQuat();
38+
4639
accel *= GRAVITY; // convert to given units
4740

4841
// Remove centrifugal
@@ -67,27 +60,29 @@ void accIntegral::updateVel( vec3_t angvel,
6760
vec3_t dvel = vel_t - vel;
6861

6962
float dvel_mag = dvel.dot(dvel);
70-
float gain_vel = VAR_COMB/(VAR_COMB + var_vel*var_acc); // gain affected by velocity and acceleration
63+
// gain affected by velocity and acceleration
64+
float gain_vel = VAR_COMB/(VAR_COMB + var_vel*var_acc);
7165

7266
var_vel = dvel_mag + gain_vel*var_vel;
7367

7468
// Integrate
75-
float dt = updateTimer();
69+
float dt = imuFilter::timeStep();
7670
vel += 0.5*(accel + accel_last)*dt; // trapezoidal rule
7771
vel += dvel*gain_vel; // update with target velocity
7872

7973
accel_last = accel;
8074
}
8175

8276
// verbose update
83-
void accIntegral::updateVel( float gx, float gy, float gz,
84-
float ax, float ay, float az,
85-
float vx, float vy, float vz,
86-
const float SD_ACC,
87-
const float SD_VEL,
88-
const float GRAVITY ) {
77+
void accIntegral::update( float gx, float gy, float gz,
78+
float ax, float ay, float az,
79+
float vx, float vy, float vz,
80+
const float SD_ACC,
81+
const float SD_VEL,
82+
const float GRAVITY,
83+
const float ALPHA ) {
8984
vec3_t angvel = {gx, gy, gz};
9085
vec3_t accel = {ax, ay, az};
9186
vec3_t vel_t = {vx, vy, vz};
92-
return updateVel( angvel, accel, vel_t, SD_ACC, SD_VEL, GRAVITY );
87+
return update( angvel, accel, vel_t, ALPHA, SD_ACC, SD_VEL, GRAVITY );
9388
}

‎src/accIntegral.h‎

Lines changed: 19 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -3,42 +3,37 @@
33
#ifndef accIntegral_h
44
#define accIntegral_h
55

6-
//------------------ Coefficients --------------------
7-
8-
#define DEFAULT_GRAVITY 9.80665 // Default gravity magnitude [ m/s^2 ]
9-
106
//---------------- Class definition ------------------
117

128
class accIntegral: public imuFilter {
139
private:
1410
// vectors
15-
vec3_t vel;
16-
vec3_t accel_mean;
17-
vec3_t accel_last;
11+
vec3_t vel = 0;
12+
vec3_t accel_mean = {0,0,0};
13+
vec3_t accel_last = {0,0,0};
1814

1915
// scalars
20-
float var_vel;
21-
float var_acc;
22-
uint32_t time_last;
16+
float var_vel = 0;
17+
float var_acc = 0;
2318

24-
// functions
25-
float updateTimer();
26-
2719
public:
2820
void reset();
21+
2922
vec3_t getVel();
3023

31-
void updateVel( vec3_t, vec3_t, vec3_t,
32-
const float,
33-
const float,
34-
const float=DEFAULT_GRAVITY );
35-
36-
void updateVel( float, float, float,
37-
float, float, float,
38-
float, float, float,
39-
const float,
40-
const float,
41-
const float=DEFAULT_GRAVITY );
24+
void update( vec3_t, vec3_t, vec3_t,
25+
const float,
26+
const float,
27+
const float,
28+
const float=DEFAULT_GAIN );
29+
30+
void update( float, float, float,
31+
float, float, float,
32+
float, float, float,
33+
const float,
34+
const float,
35+
const float,
36+
const float=DEFAULT_GAIN );
4237
};
4338

4439
#endif

‎src/imuFilter.cpp‎

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -2,11 +2,11 @@
22

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

5-
float imuFilter::updateTimer() {
5+
void imuFilter::updateTimer() {
66
uint32_t time_now = micros();
77
uint32_t change_time = time_now - last_time;
88
last_time = time_now;
9-
return float(change_time)*1e-6;
9+
dt = float(change_time)*1e-6;
1010
}
1111

1212
void imuFilter::setup() {
@@ -38,7 +38,7 @@ void imuFilter::setup( vec3_t accel ) {
3838

3939
void imuFilter::update( float gx, float gy, float gz ) {
4040
// Update Timer
41-
float dt = updateTimer();
41+
updateTimer();
4242

4343
// Rotation increment
4444
vec3_t da = vec3_t(gx, gy, gz)*dt;
@@ -56,13 +56,13 @@ void imuFilter::update( float gx, float gy, float gz,
5656
const float ALPHA /*=DEFAULT_GAIN*/,
5757
const float SD_ACC /*=DEFAULT_SD*/ ) {
5858
// Update Timer
59-
float dt = updateTimer();
59+
updateTimer();
6060

6161
// check global acceleration:
6262
vec3_t accel = {ax, ay, az};
6363
vec3_t acrel = q.rotate(accel, GLOBAL_FRAME) - vec3_t(0,0,1);
6464

65-
// kalmal filter:
65+
// kalmal filter:
6666
const float INV_VAR = 1.0/( SD_ACC * SD_ACC );
6767
float error = acrel.dot(acrel); // deviation from vertical
6868

@@ -134,6 +134,10 @@ vec3_t imuFilter::projectVector( vec3_t vec, const bool TO_WORLD ) {
134134
return q.rotate(vec, TO_WORLD);
135135
}
136136

137+
float imuFilter::timeStep() {
138+
return dt;
139+
}
140+
137141
//------------------ Euler Angles -------------------
138142

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

‎src/imuFilter.h‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -16,15 +16,16 @@ class imuFilter {
1616
private:
1717
quat_t q;
1818
float var;
19+
float dt;
1920
uint32_t last_time;
2021

21-
float updateTimer();
22+
void updateTimer();
2223

2324
public:
2425
// Initialization:
2526
void setup();
2627
void setup( float, float, float );
27-
void setup( vec3_t );
28+
void setup( vec3_t );
2829

2930
// Heading estimate:
3031
void update( float, float, float );
@@ -36,11 +37,13 @@ class imuFilter {
3637

3738
void update( vec3_t );
3839
void update( vec3_t, vec3_t,
39-
const float=DEFAULT_GAIN,
40+
const float=DEFAULT_GAIN,
4041
const float=DEFAULT_SD );
4142

4243
void rotateHeading( float, const bool );
4344

45+
float timeStep();
46+
4447
//-- Fusion outputs:
4548

4649
// Quaternion

0 commit comments

Comments
 (0)