MohamedGaberZidan commented 1 year ago

Hello,

I'm trying to run basicMPU6050 with ESP32, but I got some errors, hope you can help me with that. Firstly, for Velocity example, esp32 keep resetting. I tried to debug it and I think this error because of vector data type library. Secondly, For output example. The readings are not stable at all and keep changes although I didn't move the module at all.

Sorry for your time, I will be very grateful if you can help me. Thanks in advance.

Sincerely, Zidan

RCmags commented 1 year ago

Hey, I'll first focus on your issue with vector_datatype. We can work up from there to make all the libraries work.

RCmags commented 1 year ago

Note, please use tockn's library to retrieve the gyroscope and accelerometer value. Provide these values to the imuFilter's "update" function and run the output example.

MohamedGaberZidan commented 1 year ago

I adjusted it and used vector output example and here is the serial 10:44:10.744 -> x = -0.10,0.23,0.97 | y = 0.82,-0.53,0.22 | z = 0.56,0.82,-0.14 | v = 0.72,-0.29,1.18 | q = -0.24,0.62,0.42,0.61 10:44:10.847 -> x = -0.09,0.26,0.96 | y = 0.82,-0.52,0.22 | z = 0.56,0.81,-0.17 | v = 0.73,-0.26,1.18 | q = -0.23,0.63,0.43,0.60 10:44:10.982 -> x = -0.08,0.27,0.96 | y = 0.82,-0.54,0.21 | z = 0.57,0.80,-0.18 | v = 0.74,-0.27,1.18 | q = -0.23,0.64,0.42,0.60 10:44:11.118 -> x = -0.06,0.25,0.97 | y = 0.83,-0.53,0.19 | z = 0.56,0.81,-0.17 | v = 0.76,-0.28,1.16 | q = -0.24,0.64,0.42,0.60 10:44:11.219 -> x = -0.06,0.24,0.97 | y = 0.83,-0.53,0.18 | z = 0.56,0.81,-0.17 | v = 0.77,-0.29,1.15 | q = -0.25,0.64,0.42,0.60 10:44:11.356 -> x = -0.07,0.25,0.97 | y = 0.83,-0.53,0.19 | z = 0.56,0.81,-0.17 | v = 0.76,-0.28,1.16 | q = -0.24,0.64,0.42,0.60 10:44:11.456 -> x = -0.07,0.27,0.96 | y = 0.82,-0.54,0.21 | z = 0.57,0.80,-0.18 | v = 0.75,-0.27,1.17 | q = -0.23,0.64,0.42,0.60 10:44:11.594 -> x = -0.09,0.26,0.96 | y = 0.81,-0.54,0.22 | z = 0.57,0.80,-0.17 | v = 0.73,-0.27,1.18 | q = -0.23,0.64,0.42,0.60 10:44:11.696 -> x = -0.07,0.26,0.96 | y = 0.82,-0.53,0.20 | z = 0.56,0.81,-0.17 | v = 0.75,-0.27,1.17 | q = -0.24,0.64,0.42,0.60 10:44:11.833 -> x = -0.07,0.28,0.96 | y = 0.82,-0.53,0.21 | z = 0.57,0.80,-0.19 | v = 0.75,-0.25,1.17 | q = -0.23,0.64,0.43,0.59 10:44:11.934 -> x = -0.07,0.28,0.96 | y = 0.82,-0.53,0.21 | z = 0.56,0.80,-0.19 | v = 0.75,-0.25,1.17 | q = -0.23,0.64,0.43,0.59 10:44:12.068 -> x = -0.07,0.27,0.96 | y = 0.81,-0.55,0.21 | z = 0.58,0.79,-0.18 | v = 0.75,-0.28,1.17 | q = -0.23,0.64,0.42,0.60 10:44:12.170 -> x = -0.07,0.26,0.96 | y = 0.82,-0.53,0.20 | z = 0.57,0.80,-0.18 | v = 0.76,-0.27,1.16 | q = -0.24,0.64,0.42,0.60 10:44:12.304 -> x = -0.05,0.25,0.97 | y = 0.83,-0.53,0.18 | z = 0.56,0.81,-0.18 | v = 0.77,-0.28,1.15 | q = -0.24,0.64,0.42,0.59 10:44:12.406 -> x = -0.06,0.24,0.97 | y = 0.83,-0.53,0.18 | z = 0.56,0.81,-0.17 | v = 0.77,-0.29,1.15 | q = -0.25,0.64,0.42,0.60 10:44:12.541 -> x = -0.05,0.24,0.97 | y = 0.82,-0.54,0.18 | z = 0.57,0.81,-0.17 | v = 0.77,-0.30,1.15 | q = -0.25,0.64,0.41,0.60 10:44:12.642 -> x = -0.05,0.24,0.97 | y = 0.81,-0.56,0.18 | z = 0.59,0.79,-0.16 | v = 0.76,-0.33,1.15 | q = -0.24,0.65,0.40,0.60 10:44:12.780 -> x = -0.05,0.20,0.98 | y = 0.81,-0.57,0.16 | z = 0.59,0.80,-0.13 | v = 0.76,-0.37,1.14 | q = -0.25,0.64,0.39,0.61 10:44:12.883 -> x = -0.06,0.19,0.98 | y = 0.81,-0.56,0.16 | z = 0.58,0.81,-0.12 | v = 0.75,-0.37,1.14 | q = -0.25,0.64,0.39,0.61

##############and this the log from vector output without and modifications

10:47:05.800 -> x = 0.01,0.23,-0.97 | y = 0.11,0.97,0.23 | z = 0.99,-0.11,-0.01 | v = 0.12,1.20,-0.74 | q = 0.70,0.12,0.70,0.04 10:47:05.835 -> x = 0.01,0.23,-0.97 | y = 0.10,0.97,0.23 | z = 0.99,-0.10,-0.01 | v = 0.11,1.20,-0.74 | q = 0.70,0.12,0.70,0.05 10:47:05.869 -> x = 0.01,0.23,-0.97 | y = 0.09,0.97,0.23 | z = 1.00,-0.09,-0.01 | v = 0.10,1.20,-0.74 | q = 0.70,0.12,0.70,0.05 10:47:05.903 -> x = 0.01,0.23,-0.97 | y = 0.09,0.97,0.23 | z = 1.00,-0.09,-0.01 | v = 0.10,1.20,-0.74 | q = 0.70,0.11,0.70,0.05 10:47:05.936 -> x = 0.01,0.23,-0.97 | y = 0.08,0.97,0.23 | z = 1.00,-0.08,-0.01 | v = 0.09,1.20,-0.74 | q = 0.70,0.11,0.70,0.05 10:47:05.970 -> x = 0.01,0.23,-0.97 | y = 0.08,0.97,0.23 | z = 1.00,-0.08,-0.01 | v = 0.09,1.20,-0.74 | q = 0.70,0.11,0.70,0.05 10:47:05.970 -> x = 0.01,0.23,-0.97 | y = 0.07,0.97,0.23 | z = 1.00,-0.07,-0.01 | v = 0.08,1.20,-0.74 | q = 0.70,0.11,0.70,0.06 10:47:06.004 -> x = 0.01,0.23,-0.97 | y = 0.07,0.97,0.23 | z = 1.00,-0.07,-0.01 | v = 0.08,1.20,-0.74 | q = 0.70,0.11,0.70,0.06 10:47:06.038 -> x = 0.01,0.23,-0.97 | y = 0.06,0.97,0.23 | z = 1.00,-0.06,-0.00 | v = 0.07,1.20,-0.74 | q = 0.70,0.10,0.70,0.06 10:47:06.073 -> x = 0.01,0.23,-0.97 | y = 0.06,0.97,0.23 | z = 1.00,-0.06,-0.00 | v = 0.07,1.20,-0.74 | q = 0.70,0.10,0.70,0.06 10:47:06.108 -> x = 0.01,0.23,-0.97 | y = 0.05,0.97,0.23 | z = 1.00,-0.05,-0.00 | v = 0.06,1.20,-0.74 | q = 0.70,0.10,0.70,0.06 10:47:06.141 -> x = 0.01,0.23,-0.97 | y = 0.05,0.97,0.23 | z = 1.00,-0.05,-0.00 | v = 0.06,1.20,-0.74 | q = 0.70,0.10,0.70,0.07 10:47:06.176 -> x = 0.01,0.23,-0.97 | y = 0.04,0.97,0.23 | z = 1.00,-0.04,0.00 | v = 0.05,1.20,-0.74 | q = 0.70,0.10,0.70,0.07 10:47:06.176 -> x = 0.01,0.23,-0.97 | y = 0.03,0.97,0.23 | z = 1.00,-0.04,0.00 | v = 0.05,1.20,-0.74 | q = 0.70,0.09,0.70,0.07 10:47:06.209 -> x = 0.01,0.23,-0.97 | y = 0.03,0.97,0.23 | z = 1.00,-0.03,0.00 | v = 0.04,1.20,-0.74 | q = 0.70,0.09,0.70,0.07 10:47:06.244 -> x = 0.01,0.23,-0.97 | y = 0.02,0.97,0.23 | z = 1.00,-0.03,0.00 | v = 0.03,1.20,-0.74 | q = 0.70,0.09,0.70,0.07 10:47:06.279 -> x = 0.01,0.23,-0.97 | y = 0.02,0.97,0.23 | z = 1.00,-0.02,0.01 | v = 0.03,1.20,-0.74 | q = 0.71,0.09,0.70,0.07 10:47:06.313 -> x = 0.01,0.23,-0.97 | y = 0.27,0.94,0.22 | z = 0.96,-0.26,-0.05 | v = 0.28,1.17,-0.75 | q = 0.69,0.18,0.70,-0.01 10:47:06.347 -> x = 0.01,0.23,-0.97 | y = 0.26,0.94,0.22 | z = 0.96,-0.26,-0.05 | v = 0.27,1.17,-0.75 | q = 0.69,0.17,0.70,-0.01 10:47:06.381 -> x = 0.01,0.23,-0.97 | y = 0.26,0.94,0.22 | z = 0.97,-0.25,-0.05 | v = 0.27,1.17,-0.75 | q = 0.69,0.17,0.70,-0.01

I think both working good now, Still has the velocity example problem

RCmags commented 1 year ago

Please provide a console log of the velocity example. I suspect the values are overflowing and this is causing a reset. Although, since the heading estimate is working, I'm not sure why the velocity integration continues to fail.

MohamedGaberZidan commented 1 year ago

Thank you so much for your support. Here is the log 0:08:14.994 -> Rebooting... 00:08:14.994 -> ets Jun 8 2016 00:22:57 00:08:14.994 -> 00:08:14.994 -> rst:0xc (SW_CPU_RESET),boot:0x13 (SPI_FAST_FLASH_BOOT) 00:08:14.994 -> configsip: 0, SPIWP:0xee 00:08:14.994 -> clk_drv:0x00,q_drv:0x00,d_drv:0x00,cs0_drv:0x00,hd_drv:0x00,wp_drv:0x00 00:08:15.028 -> mode:DIO, clock div:1 00:08:15.028 -> load:0x3fff0018,len:4 00:08:15.028 -> load:0x3fff001c,len:1216 00:08:15.028 -> ho 0 tail 12 room 4 00:08:15.028 -> load:0x40078000,len:10944 00:08:15.028 -> load:0x40080400,len:6388 00:08:15.028 -> entry 0x400806b4 00:08:15.096 -> Guru Meditation Error: Core 0 panic'ed (LoadProhibited). Exception was unhandled. 00:08:15.131 -> Core 0 register dump: 00:08:15.131 -> PC : 0x400eb0df PS : 0x00060330 A0 : 0x800d0d0f A1 : 0x3ffe3b70
00:08:15.131 -> A2 : 0x3ffbfedc A3 : 0x00000000 A4 : 0x00000000 A5 : 0x00000000
00:08:15.131 -> A6 : 0x00000000 A7 : 0x00000000 A8 : 0x00000000 A9 : 0x00000000
00:08:15.131 -> A10 : 0x3ffe3b64 A11 : 0x00000000 A12 : 0x00000000 A13 : 0x00000000
00:08:15.165 -> A14 : 0x00ff0000 A15 : 0xff000000 SAR : 0x0000001d EXCCAUSE: 0x0000001c
00:08:15.165 -> EXCVADDR: 0x00000000 LBEG : 0x4000142d LEND : 0x4000143a LCOUNT : 0xfffffff8
00:08:15.165 -> 00:08:15.165 -> ELF file SHA256: 0000000000000000 00:08:15.165 -> 00:08:15.165 -> Backtrace: 0x400eb0df:0x3ffe3b70 0x400d0d0c:0x3ffe3b90 0x400e13bf:0x3ffe3bb0 0x40081ae1:0x3ffe3bd0 0x40081ce1:0x3ffe3c20 0x4007925f:0x3ffe3c40 0x400792c5:0x3ffe3c70 0x400792d0:0x3ffe3ca0 0x4007947d:0x3ffe3cc0 0x400806e6:0x3ffe3df0 0x40007c31:0x3ffe3eb0 0x4000073d:0x3ffe3f20 00:08:15.200 ->

######### And here is the Esp decoder it has a problem in vec3_t::vec3_t(float*)

PC: 0x400eb0df: vec3_t::vec3_t(float*) at C:\Users\mohamedalaa\Documents\Arduino\libraries\vector_datatype-main\src\vector_type.cpp line 23 EXCVADDR: 0x00000000

Decoding stack results 0x400eb0df: vec3_t::vec3_t(float*) at C:\Users\mohamedalaa\Documents\Arduino\libraries\vector_datatype-main\src\vector_type.cpp line 23 0x400d0d0c: _GLOBAL__sub_I_fusion() at C:\Users\mohamedalaa\Documents\Arduino\libraries\imuFilter-main\src/accIntegral.h line 12 0x400e13bf: do_global_ctors at /home/runner/work/esp32-arduino-lib-builder/esp32-arduino-lib-builder/esp-idf/components/esp32/cpu_start.c line 510 0x40081ae1: start_cpu0_default at /home/runner/work/esp32-arduino-lib-builder/esp32-arduino-lib-builder/esp-idf/components/esp32/cpu_start.c line 396 0x40081ce1: call_start_cpu0 at /home/runner/work/esp32-arduino-lib-builder/esp32-arduino-lib-builder/esp-idf/components/esp32/cpu_start.c line 262

I changed this line vec3_t vel = 0 to vec3_t vel ={0,0,0}; and the example working now but the values are very bad and unstable

00:54:03.557 -> 0.00 4.20 84.72 00:54:04.554 -> 6949.02 -17682.68 -12039.19 00:54:05.548 -> 8436.21 -81009.53 -18027.19 00:54:06.575 -> 23896.87 -92098.23 -78620.74 00:54:07.567 -> 68766.63 630.80 -254356.56 00:54:08.562 -> 102568.37 306132.81 -385416.25 00:54:09.586 -> 44407.03 943191.06 -153986.11 00:54:10.577 -> -239259.25 1466682.25 963422.38 00:54:11.600 -> 805413.06 5007426.50 -3119825.50 00:54:12.587 -> 921158.50 11454403.00 -3525156.00 00:54:13.612 -> -2860170.50 17904964.00 11403406.00 00:54:14.598 -> -9637048.00 10277900.00 38040568.00 00:54:15.620 -> -4363191.50 134596400.00 20050594.00 00:54:16.612 -> -25820270.00 250734832.00 108206360.00 00:54:17.607 -> -114736488.00 127245976.00 458294688.00 00:54:18.626 -> -204922624.00 -419860192.00 800869248.00

RCmags commented 1 year ago

No worries, I'm glad to help :)

As for the code, according this reddit thread the error:

Guru Meditation Error: Core 0 panic'ed (LoadProhibited). Exception was unhandled.

Is likely due to reading an incorrect pointer. This would definately be the case if you were defining vel_t as a float!

vec3_t vel = 0;

The above statement is incorrect and should lead to a compilation error. A 3D vector must be defined with 3 values, as you did later:

vec3_t vel ={0,0,0};

If the code has compiled sucesfully at this point, and the ESP doesn't reset, then we're much closer to getting a proper result. Please confirm if this is the case.

The exponentially growing numbers tells us the velocity isn't being integrated properly. This will occur if the acceleration is not being suppressed. Can you please print the acceleration output, along with the velocity estimate?

Something like this will do:

Serial.print(" ax = "); Serial.print(ax);
Serial.print(" ay = "); Serial.print(ay);
Serial.print(" az = "); Serial.print(az);
Serial.println();

Also, please post the code you're uploading to the esp32. Thanks.

MohamedGaberZidan commented 1 year ago

Here is the log, 02:25:15.151 -> 25.05 3.77 -13.81 02:25:15.151 -> ax = 9099.88 ay = 1388.51 az = 4771.48 02:25:15.151 -> 26.97 4.40 -15.19 02:25:15.185 -> ax = 9105.86 ay = 1388.51 az = 4761.90 02:25:15.185 -> 0.39 1.26 -1.02 02:25:15.185 -> ax = 9109.46 ay = 1370.55 az = 4781.06 02:25:15.185 -> 0.16 1.91 -0.90 02:25:15.185 -> ax = 9140.59 ay = 1364.56 az = 4740.34 02:25:15.218 -> 0.40 2.32 -0.50 02:25:15.218 -> ax = 9144.18 ay = 1369.35 az = 4773.87 02:25:15.218 -> 0.29 0.30 0.33 02:25:15.218 -> ax = 9159.75 ay = 1377.73 az = 4776.27 02:25:15.252 -> -0.11 -0.60 -0.12 02:25:15.252 -> ax = 9119.04 ay = 1371.75 az = 4714.00 02:25:15.252 -> -0.45 -0.93 -0.09 02:25:15.252 -> ax = 9103.47 ay = 1371.75 az = 4735.55 02:25:15.286 -> -0.30 -0.81 0.56 02:25:15.286 -> ax = 9092.69 ay = 1383.72 az = 4757.11 02:25:15.286 -> -0.05 -0.87 0.65 02:25:15.286 -> ax = 9103.47 ay = 1369.35 az = 4776.27 02:25:15.319 -> 0.41 -0.93 0.22 02:25:15.319 -> ax = 9147.78 ay = 1374.14 az = 4742.74

############################And here is the code

/ This sketch shows to integrate acceleration to obtain an estimate of velocity /

/* NOTE:

The accelerometer MUST be calibrated to integrate its output. Adjust the AX, AY, AND AZ offsets until the sensor reads (0,0,GRAVITY) at rest.
REQUIRED UNITS: I. The gyroscope must output: radians/second II. The accelerometer output: length/second^2 [The length unit is arbitrary. Can be meter, feet, etc] */

basicMPU6050

include

// ========== IMU sensor ==========

// Gyro settings:

define LP_FILTER 3 // Low pass filter. Value from 0 to 6

define GYRO_SENS 0 // Gyro sensitivity. Value from 0 to 3

define ACCEL_SENS 0 // Accelerometer sensitivity. Value from 0 to 3

define ADDRESS_A0 LOW // I2C address from state of A0 pin. A0 -> GND : ADDRESS_A0 = LOW

                                    //                                     A0 -> 5v  : ADDRESS_A0 = HIGH

// Accelerometer offset: constexpr int AX_OFFSET = 552; // Use these values to calibrate the accelerometer. The sensor should output 1.0g if held level. constexpr int AY_OFFSET = -241; // These values are unlikely to be zero. constexpr int AZ_OFFSET = -3185;

//-- Set template parameters:

basicMPU6050<LP_FILTER, GYRO_SENS, ACCEL_SENS, ADDRESS_A0, AX_OFFSET, AY_OFFSET, AZ_OFFSET

imu;

// =========== Settings =========== accIntegral fusion;

// Filter coefficients // Unit
constexpr float GRAVITY = 9.81e3; // mm/s^2 Magnitude of gravity at rest. Determines units of velocity. [UNITS MUST MATCH ACCELERATION] constexpr float SD_ACC = 1000; // mm/s^2 Standard deviation of acceleration. Deviations from zero are suppressed. constexpr float SD_VEL = 200; // mm/s Standard deviation of velocity. Deviations from target value are suppressed. constexpr float ALPHA = 0.5; // Gain of heading update - See example "output" for more information.

// Sensor scaling constexpr float TO_LENGTH_PER_SECOND_SQ = GRAVITY; // Constant to convert acceleration measurements to: length/second^2 constexpr float TO_RAD_PER_SECOND = 1.0; // Constant to convery gyroscope measurements to: radians/second

void setup() { Serial.begin(38400); delay(1000);

// calibrate IMU sensor imu.setup(); imu.setBias();

// initialize sensor fusion //fusion.setup( imu.ax(), imu.ay(), imu.az() ); // ALWAYS set initial heading with acceleration fusion.setup();

//fusion.reset(); / Use this function to zero velocity estimate /
}

void loop() {
imu.updateBias();

// Measure state:
vec3_t accel = { imu.ax(), imu.ay(), imu.az() }; vec3_t gyro = { imu.gx(), imu.gy(), imu.gz() };

  // Scale measurements

accel = TO_LENGTH_PER_SECOND_SQ; gyro = TO_RAD_PER_SECOND;

// Update heading and velocity estimate:

  // known measured velocity (target state). Estimate will be forced towards this vector

vec3_t vel_t = {0,0,0};

  /* note: all coefficients are optional and have default values */

fusion.update( gyro, accel, vel_t, SD_ACC, SD_VEL, GRAVITY, ALPHA );

  // obtain velocity estimate

vec3_t vel = fusion.getVel();

// Display velocity components: [view with serial plotter] Serial.print( vel.x, 2 ); Serial.print( " " ); Serial.print( vel.y, 2 ); Serial.print( " " ); Serial.print( vel.z, 2 );
Serial.println(); Serial.print(" ax = "); Serial.print(accel.x); Serial.print(" ay = "); Serial.print(accel.y); Serial.print(" az = "); Serial.print(accel.z); Serial.println();

}

Thanks in advance.

RCmags commented 1 year ago

Great! The output looks reasonable. Set all the accelerometer offsets to zero. The sensor appeared well calibrated using tockns library.

Once you upload the code, wait 20 seconds for the velocity to self calibrate. After, use the serial plotter to view the output. When you abruptly move the sensor, you should see a velocity pulse.

MohamedGaberZidan commented 1 year ago

I tried this but unfortunately the same problem exist, here is the log

16:24:38.810 -> -21591.42 219198.39 456439.03 16:24:38.810 -> -21429.69 230841.78 451029.09 16:24:38.810 -> -21257.53 242353.48 445321.38 16:24:38.810 -> -21073.44 253747.61 439312.06 16:24:38.843 -> -20884.95 265014.88 433165.94 16:24:38.843 -> -20978.72 260829.30 435913.00 16:24:38.843 -> -21061.42 256584.83 438413.84 16:24:38.843 -> -21142.79 252221.09 440885.81 16:24:38.878 -> -21233.94 247927.88 443625.50 16:24:38.878 -> -21316.54 243515.86 446009.72 16:24:38.878 -> -21404.42 239174.16 448499.53 16:24:38.878 -> -21218.29 250549.22 442365.00 16:24:38.913 -> -21028.39 261872.16 436148.41 16:24:38.913 -> -20825.81 273033.63 429646.91 16:24:38.913 -> -20920.32 268876.81 432476.06 16:24:38.913 -> -21013.12 264678.44 435212.97 16:24:38.913 -> -20797.42 275690.59 428285.28

MohamedGaberZidan commented 1 year ago

Hello, I fixed the issue by changing this line from fusion.update( gyro, accel, vel_t, SD_ACC, SD_VEL, GRAVITY, ALPHA ); to fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() , 0,0,0, SD_ACC, SD_VEL, GRAVITY, ALPHA );

but another problem appeared that the change in speed are very low, I tried to move it very fast with my hand but It doesn't exceed 1 at all. Here is the log .

00:11:18.913 -> -0.00 0.00 0.00 00:11:18.913 -> -0.04 -0.01 -0.02 00:11:18.947 -> -0.03 -0.01 -0.02 00:11:18.947 -> 0.01 -0.01 -0.01 00:11:18.981 -> -0.00 -0.01 0.04 00:11:19.015 -> 0.01 -0.01 0.03 00:11:19.015 -> 0.01 -0.02 -0.02 00:11:19.049 -> -0.01 -0.02 -0.01 00:11:19.049 -> -0.01 -0.00 0.00 00:11:19.083 -> 0.00 0.03 -0.03 00:11:19.117 -> -0.01 0.01 -0.03 00:11:19.117 -> -0.02 -0.01 0.00 00:11:19.152 -> -0.00 0.00 -0.03 00:11:19.152 -> 0.01 -0.03 0.00 00:11:19.187 -> 0.01 -0.02 -0.00 00:11:19.187 -> 0.00 0.01 -0.04 00:11:19.221 -> -0.00 0.01 -0.01 00:11:19.256 -> -0.01 -0.01 0.01 00:11:19.256 -> -0.00 0.03 0.01 00:11:19.290 -> -0.00 0.04 0.01 00:11:19.290 -> -0.00 0.01 -0.00 00:11:19.323 -> 0.03 -0.01 -0.02 00:11:19.357 -> 0.01 -0.00 -0.03 00:11:19.357 -> -0.01 0.03 -0.00 00:11:19.390 -> 0.01 0.02 0.01 00:11:19.390 -> -0.01 0.00 -0.01 00:11:19.424 -> -0.01 -0.02 0.03 00:11:19.424 -> -0.00 0.00 0.03 00:11:19.458 -> -0.00 0.03 -0.00 00:11:19.492 -> 0.01 0.01 0.03 00:11:19.492 -> 0.02 0.01 -0.01 00:11:19.525 -> 0.02 0.00 -0.01 00:11:19.525 -> 0.01 0.00 0.02 00:11:19.559 -> -0.00 0.01 0.00 00:11:19.594 -> 0.01 -0.02 0.00 00:11:19.594 -> -0.02 -0.02 0.02 00:11:19.628 -> -0.03 0.00 0.01 00:11:19.628 -> -0.01 -0.01 0.01 00:11:19.662 -> 0.02 0.02 0.01 00:11:19.696 -> 0.02 -0.00 0.01 00:11:19.696 -> -0.01 -0.04 0.01 00:11:19.730 -> -0.00 -0.01 -0.00 00:11:19.730 -> -0.00 0.01 0.01 00:11:19.764 -> 0.01 -0.01 -0.01 00:11:19.764 -> -0.02 -0.01 -0.02 00:11:19.797 -> -0.04 0.00 -0.01 00:11:19.831 -> -0.01 -0.02 -0.01 00:11:19.831 -> 0.01 -0.01 0.03 00:11:19.865 -> 0.02 -0.01 0.03 00:11:19.865 -> -0.01 -0.02 -0.02 00:11:19.899 -> -0.01 -0.01 -0.01 00:11:19.933 -> -0.00 -0.00 0.00 00:11:19.933 -> 0.00 -0.00 0.01 00:11:19.967 -> -0.00 0.04 -0.03 00:11:19.967 -> 0.00 0.04 -0.03 00:11:20.001 -> 0.01 -0.00 0.01 00:11:20.035 -> 0.00 0.00 0.00 00:11:20.035 -> 0.00 0.00 -0.00 00:11:20.070 -> -0.00 0.00 -0.01 00:11:20.070 -> 0.00 0.00 -0.01 00:11:20.104 -> 0.00 0.01 -0.00 00:11:20.104 -> -0.03 -0.02 -0.00 00:11:20.138 -> -0.03 -0.02 -0.01 00:11:20.172 -> 0.03 0.03 -0.00 00:11:20.172 -> 0.04 0.02 0.01

Thanks in advance.

raulg-cpp commented 1 year ago

Woah! That is a weird bug! Thank you making me aware of this! If it's not blowing up, its a great sign. That means its likely integrating properly.

The output is small because you are not scaling the accelerometer values. They're still in g-units.

Change this line of code:

fusion.update( imu.gx(), imu.gy(), imu.gz(), imu.ax(), imu.ay(), imu.az() , 0,0,0, SD_ACC, SD_VEL, GRAVITY, ALPHA );

To this:

fusion.update( imu.gx(), imu.gy(), imu.gz(), 
                        imu.ax() * TO_LENGTH_PER_SECOND_SQ, 
                        imu.ay() * TO_LENGTH_PER_SECOND_SQ, 
                        imu.az() * TO_LENGTH_PER_SECOND_SQ, 
                        0,0,0, SD_ACC, SD_VEL, GRAVITY, ALPHA );

If that doesn't work, replace TO_LENGTH_PER_SECOND_SQ to 9.81e3 in the fusion() call.

Alternatively, you can change the filter coefficients to g-units. This is simpler and should work just as well, although you will need to multiply the velocity or will be very small.

constexpr float SD_ACC  = 1000 / 9.81e3;      //  0.1    |  ( m^2 / s )/( m^2 / s )  = g-unit
constexpr float SD_VEL  = 200 / 9.81e3;         // 0.020

Then scale the velocity:

vec3_t vel = fusion.getVel() * 9.81e3;  //   mm / second

MohamedGaberZidan commented 1 year ago

Hello, After doing the scaling part the readings were stable at the beginning till I move the IMU, it gave these large values again. 16:24:38.810 -> -21591.42 219198.39 456439.03 16:24:38.810 -> -21429.69 230841.78 451029.09 16:24:38.810 -> -21257.53 242353.48 445321.38 16:24:38.810 -> -21073.44 253747.61 439312.06 16:24:38.843 -> -20884.95 265014.88 433165.94 16:24:38.843 -> -20978.72 260829.30 435913.00 Thanks in advance.

RCmags commented 1 year ago

Hmm please try the alternative method wherein the you don't multiply the acceleration, but rather multiply the velocity.

You got stable values with the un-scaled acceleration, so it should continue to remain stable.

MohamedGaberZidan commented 1 year ago

If I didn't scale the Accel and gyro, isn't this wrong as it different units, And if not what will be the velocity output unit ?. Thanks in advance.

RCmags commented 1 year ago

There's two ways to scale the velocity:

You can scale the acceleration (not the gyro!! it must be in rad/s only!!), and that will raise the size of the velocity.
Multiply the velocity directly, with the same scale of the acceleration.

It comes from calculus. A constant can exist inside or outside the integration sign:

$https://latex.codecogs.com/svg.image?v = \int a x dx = a \int xdx$

It might be the bug you're experiencing is related to the acceleration being scaled.

MohamedGaberZidan commented 1 year ago

Thank you so much I did that it solved the issue, I'm trying now to get the displacement from the velocity using theselines of code but I don't get the hoped results float constant = 9800; accel_last = accel; vec3_t ddisp ={0,0,0}; ddisp = (vel constant)dt; disp += ddisp;

Do you have any recommendation?. Thanks in advance.

RCmags commented 1 year ago

Great news! We've identified one bug so far.

How are you getting dt? Try using the timeStep function included with the filter:

dt = fusion.timeStep();

The acceleration integration is only an approximation. You will need to play with the filter coeficients SD_ACC and SD_VEL to get satisfactory results.

Nonetheless, it's still not perfect. If you move the imu slowly, the filter wont measure the velocity very well (if at all). It works best with fast and abrupt displacements. For example, a box on a floor getting hit with a bat.

Here's a small tank I made using this filter. Note how I kick the imu to measure displacements.

MohamedGaberZidan commented 1 year ago

I'm adjusting the library itself accintegral.cpp file, I'm getting now a good velocity but the displacement is not good I don't know the reason for that Here is the code

vec3_t accIntegral::getDis() { return disp; } // update velocity with imu measurements void accIntegral::update( vec3_t angvel, vec3_t accel, vec3_t vel_t, const float SD_ACC, const float SD_VEL, const float GRAVITY, const float ALPHA ) { // 0. Constants: const float VAR_ACC = SD_ACCSD_ACC; const float VAR_VEL = SD_VELSD_VEL; const float VAR_COMB = VAR_ACC*VAR_VEL;

// 1. Remove acceleration bias: imuFilter::update( angvel, accel, ALPHA, SD_ACC, GRAVITY ); quat_t qt = imuFilter::getQuat();

// Remove centrifugal

// Serial.print("accel x before "); // Serial.println(accel.x ); // Serial.print("accel y before"); // Serial.println(accel.y); // Serial.print("accel z before"); vec3_t vel_local = qt.rotate(vel, LOCAL_FRAME); accel -= vel_local.cross(angvel);

// remove global gravity vector

accel = qt.rotate(accel, GLOBAL_FRAME); accel.z -= GRAVITY;

// Update variance

accel -= accel_mean; // Serial.print("accel x"); // Serial.println(accel.x); // Serial.print("accel y"); // Serial.println(accel.y); // Serial.print("accel z"); // Serial.println(accel.z); long acc_err = accel.dot(accel); float gain_acc = VAR_ACC/(VAR_ACC + var_acc); // Serial.print("acc_err"); // Serial.println(acc_err); // Serial.print("gain_acc"); // Serial.println(gain_acc);

var_acc = acc_err + gain_accvar_acc;
accel_mean += accelgain_acc; // Serial.print("var_acc"); // Serial.println(var_acc); // Serial.print("accel_mean x"); // Serial.println(accel_mean.x); // Serial.print("accel_mean y"); // Serial.println(accel_mean.y); // Serial.print("accel_mean z"); // Serial.println(accel_mean.z); // 2. Integrate acceleration: // Update Variance vec3_t dvel = vel_t - vel;

float dvel_mag = dvel.dot(dvel); // gain affected by velocity and acceleration float gain_vel = VAR_COMB/(VAR_COMB + var_velvar_acc); // Serial.print("dvel x"); // Serial.println(dvel.x); // Serial.print("dvel y"); // Serial.println(dvel.y); // Serial.print("dvel z"); // Serial.println(dvel.z); // Serial.print("dvel_mag"); // Serial.println(dvel_mag); var_vel = dvel_mag + gain_velvar_vel;

// Integrate

float dt = imuFilter::timeStep();

vel += 0.5(accel + accel_last)dt; // trapezoidal rule vel += dvel*gain_vel; // update with target velocity float constant = 9800; accel_last = accel;

//// getting displacement vec3_t ddisp ={0,0,0}; ddisp = (velconstant)dt; disp += ddisp;

} Thanks in advance.

RCmags commented 1 year ago

You can simplify expresion by using vel directly:

disp +=(vel*9.81e3)*dt;

Is disp defined inside the class as another member variable? It looks like it is. What value is it initialized to?

MohamedGaberZidan commented 1 year ago

Yes, it's class accIntegral: public imuFilter { private: // vectors

vec3_t   disp = { 0, 0, 0};       
vec3_t   vel = { 0, 0, 0};        
vec3_t   accel_mean = {0,0,0};     
vec3_t   accel_last = {0,0,0};

// scalars
long    var_vel = 0;       
long    var_acc = 0;

RCmags commented 1 year ago

Hmm it should work. Why do you say the position doesnt integrate well? How does it behave?

Please post a console log of the position.

MohamedGaberZidan commented 1 year ago

Yes, but I'm updating it vec3_t ddisp ={0,0,0}; ddisp = (velconstant)dt; disp += ddisp; // here I'm adding the result

here is my log 2:04:42.577 -> Vy 0.33 Dy 26.92 02:04:42.577 -> Vy 4.43 Dy 26.94 02:04:42.577 -> Vy 4.43 Dy 26.97 02:04:42.577 -> Vy 0.19 Dy 26.97 02:04:42.577 -> Vy -3.74 Dy 26.95 02:04:42.577 -> Vy -3.58 Dy 26.93 02:04:42.616 -> Vy 0.30 Dy 26.93 02:04:42.616 -> Vy 4.35 Dy 26.95 02:04:42.616 -> Vy 0.29 Dy 26.96 02:04:42.616 -> Vy 0.33 Dy 26.96 02:04:42.616 -> Vy 0.41 Dy 26.96 02:04:42.616 -> Vy -3.67 Dy 26.94 02:04:42.616 -> Vy -3.71 Dy 26.92 02:04:42.650 -> Vy -3.75 Dy 26.90 02:04:42.650 -> Vy 0.31 Dy 26.90 02:04:42.650 -> Vy 0.32 Dy 26.90 02:04:42.650 -> Vy -3.55 Dy 26.88 02:04:42.650 -> Vy 0.35 Dy 26.88 02:04:42.689 -> Vy 4.28 Dy 26.91 02:04:42.689 -> Vy 0.26 Dy 26.91 02:04:42.689 -> Vy 0.23 Dy 26.91 02:04:42.689 -> Vy 4.53 Dy 26.93 02:04:42.689 -> Vy 0.45 Dy 26.94 02:04:42.689 -> Vy -3.76 Dy 26.92 02:04:42.689 -> Vy -3.70 Dy 26.90 02:04:42.712 -> Vy 0.28 Dy 26.90 02:04:42.712 -> Vy 4.35 Dy 26.92 02:04:42.712 -> Vy 4.54 Dy 26.95 02:04:42.712 -> Vy 4.62 Dy 26.97 02:04:42.712 -> Vy 4.59 Dy 27.00 02:04:42.746 -> Vy 4.61 Dy 27.02 02:04:42.746 -> Vy 4.67 Dy 27.05 02:04:42.746 -> Vy 4.82 Dy 27.08 02:04:42.746 -> Vy 0.29 Dy 27.08 02:04:42.746 -> Vy -4.20 Dy 27.06 02:04:42.746 -> Vy -3.92 Dy 27.03 02:04:42.780 -> Vy -3.93 Dy 27.01 02:04:42.780 -> Vy 0.30 Dy 27.01 02:04:42.780 -> Vy 4.64 Dy 27.04 02:04:42.780 -> Vy 4.76 Dy 27.07 02:04:42.780 -> Vy 4.96 Dy 27.09 02:04:42.780 -> Vy 4.89 Dy 27.12 02:04:42.813 -> Vy 0.33 Dy 27.12 02:04:42.813 -> Vy -4.18 Dy 27.10 02:04:42.813 -> Vy -4.20 Dy 27.08 02:04:42.813 -> Vy -4.11 Dy 27.05 02:04:42.813 -> Vy 0.19 Dy 27.05 02:04:42.813 -> Vy 4.77 Dy 27.08 02:04:42.846 -> Vy 0.43 Dy 27.08 02:04:42.846 -> Vy -4.06 Dy 27.06 02:04:42.846 -> Vy -3.92 Dy 27.04 02:04:42.846 -> Vy -4.05 Dy 27.02 02:04:42.846 -> Vy -4.01 Dy 26.99 02:04:42.846 -> Vy -3.93 Dy 26.97 02:04:42.880 -> Vy -3.95 Dy 26.95 02:04:42.880 -> Vy -3.79 Dy 26.93 02:04:42.880 -> Vy -3.79 Dy 26.91 02:04:42.880 -> Vy -3.84 Dy 26.88 02:04:42.880 -> Vy -3.67 Dy 26.86 02:04:42.880 -> Vy -3.55 Dy 26.84 02:04:42.914 -> Vy 0.26 Dy 26.85 02:04:42.914 -> Vy 4.21 Dy 26.87 02:04:42.914 -> Vy 4.53 Dy 26.89 02:04:42.914 -> Vy 0.33 Dy 26.90 02:04:42.914 -> Vy -3.89 Dy 26.87 02:04:42.914 -> Vy -3.58 Dy 26.85 02:04:42.948 -> Vy -3.49 Dy 26.83 02:04:42.948 -> Vy -3.42 Dy 26.82 02:04:42.948 -> Vy -3.43 Dy 26.80 02:04:42.948 -> Vy -3.50 Dy 26.78 02:04:42.948 -> Vy -3.47 Dy 26.76 02:04:42.948 -> Vy -3.28 Dy 26.74 02:04:42.982 -> Vy -3.12 Dy 26.72 02:04:42.982 -> Vy -3.09 Dy 26.70 02:04:42.982 -> Vy -3.09 Dy 26.69 02:04:42.982 -> Vy -3.00 Dy 26.67 02:04:42.982 -> Vy -2.98 Dy 26.65 02:04:42.982 -> Vy -2.90 Dy 26.64 02:04:43.015 -> Vy -2.72 Dy 26.62 02:04:43.015 -> Vy -2.73 Dy 26.61 02:04:43.015 -> Vy -2.68 Dy 26.59 02:04:43.015 -> Vy 0.13 Dy 26.59 02:04:43.015 -> Vy 3.06 Dy 26.61 02:04:43.015 -> Vy 3.37 Dy 26.63 02:04:43.049 -> Vy 3.55 Dy 26.65 02:04:43.049 -> Vy 0.21 Dy 26.65 02:04:43.049 -> Vy -2.94 Dy 26.63 02:04:43.049 -> Vy -2.71 Dy 26.62 02:04:43.049 -> Vy -2.67 Dy 26.60 02:04:43.049 -> Vy 0.23 Dy 26.60 02:04:43.084 -> Vy 0.31 Dy 26.61 02:04:43.084 -> Vy 0.33 Dy 26.61 02:04:43.084 -> Vy 3.23 Dy 26.63 02:04:43.084 -> Vy 3.41 Dy 26.64 02:04:43.084 -> Vy 3.64 Dy 26.67 02:04:43.084 -> Vy 0.19 Dy 26.67 02:04:43.117 -> Vy -2.96 Dy 26.65 02:04:43.117 -> Vy -2.73 Dy 26.63 02:04:43.117 -> Vy 0.27 Dy 26.64 02:04:43.117 -> Vy 3.51 Dy 26.66 02:04:43.117 -> Vy 3.52 Dy 26.68 02:04:43.117 -> Vy 0.22 Dy 26.68 02:04:43.150 -> Vy -2.98 Dy 26.66 02:04:43.150 -> Vy 0.18 Dy 26.66 02:04:43.150 -> Vy 3.51 Dy 26.68 02:04:43.150 -> Vy 0.35 Dy 26.68 02:04:43.150 -> Vy 0.42 Dy 26.68 02:04:43.150 -> Vy 3.84 Dy 26.71 02:04:43.184 -> Vy 0.17 Dy 26.71 02:04:43.184 -> Vy 0.28 Dy 26.71 02:04:43.184 -> Vy 3.97 Dy 26.73 02:04:43.184 -> Vy 4.02 Dy 26.75 02:04:43.184 -> Vy 4.20 Dy 26.78 02:04:43.184 -> Vy 0.21 Dy 26.78 02:04:43.219 -> Vy -3.47 Dy 26.76 02:04:43.219 -> Vy -3.18 Dy 26.74 02:04:43.219 -> Vy -3.30 Dy 26.72 02:04:43.219 -> Vy -3.24 Dy 26.70 02:04:43.219 -> Vy -3.18 Dy 26.69 02:04:43.219 -> Vy 0.23 Dy 26.69 02:04:43.253 -> Vy 3.82 Dy 26.71 02:04:43.253 -> Vy 0.25 Dy 26.71 02:04:43.253 -> Vy -3.25 Dy 26.69 02:04:43.253 -> Vy -2.99 Dy 26.68 02:04:43.253 -> Vy -2.95 Dy 26.66 02:04:43.253 -> Vy -2.91 Dy 26.64 02:04:43.287 -> Vy -2.83 Dy 26.63 02:04:43.287 -> Vy 0.21 Dy 26.63 02:04:43.287 -> Vy 0.26 Dy 26.63 02:04:43.287 -> Vy 0.34 Dy 26.63 02:04:43.287 -> Vy 0.17 Dy 26.63 02:04:43.287 -> Vy -2.86 Dy 26.62 02:04:43.287 -> Vy 0.22 Dy 26.62 02:04:43.321 -> Vy 3.47 Dy 26.64 02:04:43.321 -> Vy 3.84 Dy 26.66 02:04:43.321 -> Vy 3.71 Dy 26.68 02:04:43.321 -> Vy 0.29 Dy 26.68 02:04:43.321 -> Vy 0.14 Dy 26.68 02:04:43.321 -> Vy 3.70 Dy 26.70 02:04:43.355 -> Vy 4.08 Dy 26.72 02:04:43.355 -> Vy 4.10 Dy 26.75 02:04:43.355 -> Vy 4.15 Dy 26.77 02:04:43.355 -> Vy 4.23 Dy 26.79 02:04:43.355 -> Vy 4.27 Dy 26.82 02:04:43.355 -> Vy 0.25 Dy 26.82 02:04:43.389 -> Vy -3.60 Dy 26.80 02:04:43.389 -> Vy -3.55 Dy 26.78 02:04:43.389 -> Vy 68.28 Dy 27.16 02:04:43.389 -> Vy 70.80 Dy 27.56 02:04:43.389 -> Vy -74.33 Dy 27.14 02:04:43.389 -> Vy -78.69 Dy 26.70 02:04:43.423 -> Vy 0.39 Dy 26.70 02:04:43.423 -> Vy 1.13 Dy 26.71 02:04:43.423 -> Vy -0.59 Dy 26.71 02:04:43.423 -> Vy 0.41 Dy 26.71 02:04:43.423 -> Vy 0.30 Dy 26.71 02:04:43.423 -> Vy 3.98 Dy 26.73 02:04:43.458 -> Vy 4.07 Dy 26.76 02:04:43.458 -> Vy 4.19 Dy 26.78 02:04:43.458 -> Vy 70.42 Dy 27.17 02:04:43.458 -> Vy 3.23 Dy 27.19 02:04:43.458 -> Vy -65.86 Dy 26.82 02:04:43.458 -> Vy -3.50 Dy 26.80 02:04:43.493 -> Vy -3.35 Dy 26.78 02:04:43.493 -> Vy -2.62 Dy 26.77 02:04:43.493 -> Vy -3.58 Dy 26.75 02:04:43.493 -> Vy 71.47 Dy 27.15 02:04:43.493 -> Vy -2.46 Dy 27.14 02:04:43.493 -> Vy -77.98 Dy 26.70 02:04:43.527 -> Vy -4.90 Dy 26.67 02:04:43.527 -> Vy -3.12 Dy 26.66 02:04:43.527 -> Vy -2.17 Dy 26.64 02:04:43.527 -> Vy -3.09 Dy 26.63 02:04:43.527 -> Vy -2.97 Dy 26.61 02:04:43.527 -> Vy -2.72 Dy 26.59 02:04:43.560 -> Vy 85.77 Dy 27.07 02:04:43.560 -> Vy 87.73 Dy 27.56 02:04:43.560 -> Vy -90.29 Dy 27.06 02:04:43.560 -> Vy -1.35 Dy 27.05 02:04:43.560 -> Vy -2.95 Dy 27.04 02:04:43.560 -> Vy -93.77 Dy 26.51 02:04:43.594 -> Vy 0.76 Dy 26.52 02:04:43.594 -> Vy 0.06 Dy 26.52 02:04:43.594 -> Vy -0.39 Dy 26.51 02:04:43.594 -> Vy 0.33 Dy 26.52 02:04:43.594 -> Vy -1.89 Dy 26.50 02:04:43.594 -> Vy -1.82 Dy 26.49 02:04:43.628 -> Vy -1.89 Dy 26.48 02:04:43.628 -> Vy -2.01 Dy 26.47 02:04:43.628 -> Vy 97.25 Dy 27.02 02:04:43.628 -> Vy -1.38 Dy 27.01 02:04:43.628 -> Vy -100.93 Dy 26.44 Thanks in advance.

RCmags commented 1 year ago

Umm, if the position barely changes with velocity, how exactly did it get to 26?

MohamedGaberZidan commented 1 year ago

That's confuse me, I think at the starting point it got it or something, at the first time of filtering. I really don't know :(.

RCmags commented 1 year ago

Hmm okay. Try this. Cut the displacement code and put it outside the library into the sketch. Disp should be a global variable placed outside loop().

Afterwords, load the code to esp32, and open the console plotter. Take a picture of the plot a second or two after startup. Dont move the imu.

Next, move the imu a few times, and take a picture of the plotter.

Post both pictures.

MohamedGaberZidan commented 1 year ago

Here is it, first 2 seconds

moving the IMU

RCmags commented 1 year ago

Gosh! Sorry.

Please use the serial plotter, to obtain a graph of the signals.

Also, by the looks of it, the velccity is still messed up. Its integrating wildly to upon initialization. It should not.

Did you change SD_ACC and SD_VEL to g-units?

MohamedGaberZidan commented 1 year ago

Here is it sorry,

RCmags commented 1 year ago

So the offset in position can be explained by the giant pulse at the start. Its stillvery unsensitive to velocity though.

Please read my updated last post.

MohamedGaberZidan commented 1 year ago

No, I didn't constexpr float GRAVITY = 9.81e3; // mm/s^2 Magnitude of gravity at rest. Determines units of velocity. [UNITS MUST MATCH ACCELERATION] constexpr float SD_ACC = 1000; // mm/s^2 Standard deviation of acceleration. Deviations from zero are suppressed. constexpr float SD_VEL = 200; // mm/s Standard deviation of velocity. Deviations from target value are suppressed.

RCmags commented 1 year ago

Ok. Divide the gains by gravity. That should have a significant effect.

constexpr float SD_ACC = 1000 / 9.81e3;

And

constexpr float SD_VEL = 200 / 9.81e3;

Tell me what effect that has.

MohamedGaberZidan commented 1 year ago

Actually, It didn't go well at all.

RCmags commented 1 year ago

My bad. Please set gravity to 1. constexpr float GRAVITY =1.0;

Leave the rest of the code as it was up to this step.

MohamedGaberZidan commented 1 year ago

The log is here, It does not respond at all 03:55:13.195 -> Vy -0.00 Dy 0.00 03:55:13.195 -> Vy 0.01 Dy 0.00 03:55:13.195 -> Vy 0.01 Dy 0.00 03:55:13.195 -> Vy -0.01 Dy 0.00 03:55:13.195 -> Vy -0.00 Dy 0.00 03:55:13.195 -> Vy 0.01 Dy 0.00 03:55:13.220 -> Vy -0.00 Dy 0.00 03:55:13.220 -> Vy -0.00 Dy 0.00 03:55:13.220 -> Vy -0.01 Dy 0.00 03:55:13.220 -> Vy -0.01 Dy 0.00

the code

/ This sketch shows to integrate acceleration to obtain an estimate of velocity /

/* NOTE:

The accelerometer MUST be calibrated to integrate its output. Adjust the AX, AY, AND AZ offsets until the sensor reads (0,0,GRAVITY) at rest.
REQUIRED UNITS: I. The gyroscope must output: radians/second II. The accelerometer output: length/second^2 [The length unit is arbitrary. Can be meter, feet, etc] */

include // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050

include

// ========== IMU sensor ==========

// Gyro settings:

define LP_FILTER 3 // Low pass filter. Value from 0 to 6

define GYRO_SENS 0 // Gyro sensitivity. Value from 0 to 3

define ACCEL_SENS 0 // Accelerometer sensitivity. Value from 0 to 3

define ADDRESS_A0 LOW // I2C address from state of A0 pin. A0 -> GND : ADDRESS_A0 = LOW

                                    //                                     A0 -> 5v  : ADDRESS_A0 = HIGH

// Accelerometer offset: constexpr int AX_OFFSET = 0; // Use these values to calibrate the accelerometer. The sensor should output 1.0g if held level. constexpr int AY_OFFSET = 0; // These values are unlikely to be zero. constexpr int AZ_OFFSET = 0;

//-- Set template parameters:

basicMPU6050<LP_FILTER, GYRO_SENS, ACCEL_SENS, ADDRESS_A0, AX_OFFSET, AY_OFFSET, AZ_OFFSET

imu;

// =========== Settings =========== accIntegral fusion;

// Filter coefficients // Unit
constexpr float GRAVITY = 1; // mm/s^2 Magnitude of gravity at rest. Determines units of velocity. [UNITS MUST MATCH ACCELERATION] constexpr float SD_ACC = 1000/GRAVITY; // mm/s^2 Standard deviation of acceleration. Deviations from zero are suppressed. constexpr float SD_VEL = 200/GRAVITY; // mm/s Standard deviation of velocity. Deviations from target value are suppressed. constexpr float ALPHA = .5; // Gain of heading update - See example "output" for more information.

// Sensor scaling constexpr float TO_LENGTH_PER_SECOND_SQ = GRAVITY; // Constant to convert acceleration measurements to: length/second^2 constexpr float TO_RAD_PER_SECOND = 1.0; // Constant to convery gyroscope measurements to: radians/second vec3_t disp = { 0, 0, 0 };
void setup() { Serial.begin(38400); delay(1000);

// calibrate IMU sensor imu.setup(); imu.setBias();

// initialize sensor fusion //fusion.setup( imu.ax(), imu.ay(), imu.az() ); // ALWAYS set initial heading with acceleration fusion.setup();

//fusion.reset(); / Use this function to zero velocity estimate /

}

void loop() {
imu.updateBias();

// Measure state:
vec3_t accel = { imu.ax(), imu.ay(), imu.az() }; vec3_t gyro = { imu.gx(), imu.gy(), imu.gz() };

  // Scale measurements

// accel = TO_LENGTH_PER_SECOND_SQ; gyro = TO_RAD_PER_SECOND;

// Update heading and velocity estimate:

  // known measured velocity (target state). Estimate will be forced towards this vector

vec3_t vel_t = {0,0,0};
/ note: all coefficients are optional and have default values / fusion.update( gyro, accel, vel_t, SD_ACC, SD_VEL, GRAVITY, ALPHA );

  // obtain velocity estimate

vec3_t vel = fusion.getVel();

// Serial.print( " before" ); // Serial.print( vel.x, 2 ); // Serial.print( " " ); // Serial.print( vel.y, 2 ); vel *= TO_LENGTH_PER_SECOND_SQ; Serial.print( " Vy " ); Serial.print( vel.y, 2 );

// Display velocity components: [view with serial plotter] vec3_t ddisp ={0,0,0}; ddisp = vel * fusion.timeStep(); disp += ddisp; Serial.print( " Dy " ); Serial.println( disp.y, 2 ); // Serial.print( " " ); // Serial.print( vel.y, 2 ); // Serial.print( "DZ " ); // Serial.print( disp.z );
// Serial.println();

} Should I change this line vel *= TO_LENGTH_PER_SECOND_SQ; to 9800

RCmags commented 1 year ago

Just change these lines:

constexpr float SD_ACC = 1000/9.81e3; 
constexpr float SD_VEL = 200/9.81e3; 
constexpr float TO_LENGTH_PER_SECOND_SQ = 9.81e3;

mittlc commented 8 months ago

Hello @RCmags , @MohamedGaberZidan , could you please point me to the part of this post which fixed the resetting issue? I think i am facing the same error on my esp32 - while the output example shows stable values, i cannot get the velocity example to work, the esp32 keeps resetting. I am using the imuFilter library version 1.6.2

output example -0.01 -0.05 0.01 0.004159 -0.01 -0.05 0.01 0.004152 -0.01 -0.05 0.00 0.004135

tilted by 90° on Pitch -1.48 -0.19 0.19 0.004130 -1.48 -0.19 0.19 0.004147 -1.48 -0.19 0.19 0.004152

tilted by 90° on Roll -0.03 -0.30 1.48 0.004141 -0.03 -0.30 1.48 0.004146 -0.03 -0.30 1.48 0.004145 -0.03 -0.30 1.48 0.004159 -0.03 -0.30 1.48 0.004145 -0.03 -0.30 1.48 0.004131

Vector Output Example: x = 1.00,-0.00,-0.02 | y = 0.00,1.00,-0.00 | z = 0.02,0.00,1.00 | v = 1.00,1.00,-0.03 | q = 1.00,-0.00,0.01,-0.00 x = 1.00,-0.00,-0.02 | y = 0.00,1.00,-0.00 | z = 0.02,0.00,1.00 | v = 1.00,1.00,-0.03 | q = 1.00,-0.00,0.01,-0.00 x = 1.00,-0.00,-0.02 | y = 0.00,1.00,-0.00 | z = 0.02,0.00,1.00 | v = 1.00,1.00,-0.03 | q = 1.00,-0.00,0.01,-0.00 x = 1.00,-0.00,-0.02 | y = 0.00,1.00,-0.00 | z = 0.02,0.00,1.00 | v = 1.00,1.00,-0.03 | q = 1.00,-0.00,0.01,-0.00

Velocity Example:

E (198) esp_core_dump_flash: Core dump flash config is corrupted! CRC=0x7bd5c66f instead of 0x0 Rebooting... ets Jul 29 2019 12:21:46

rst:0xc (SW_CPU_RESET),boot:0x13 (SPI_FAST_FLASH_BOOT) configsip: 0, SPIWP:0xee clk_drv:0x00,q_drv:0x00,d_drv:0x00,cs0_drv:0x00,hd_drv:0x00,wp_drv:0x00 mode:DIO, clock div:1 load:0x3fff0030,len:1184 load:0x40078000,len:13260 load:0x40080400,len:3028 entry 0x400805e4 Guru Meditation Error: Core 0 panic'ed (LoadProhibited). Exception was unhandled.

Core 0 register dump: PC : 0x400f3093 PS : 0x00060d30 A0 : 0x800d14ff A1 : 0x3ffe3be0
A2 : 0x3ffc1c74 A3 : 0x00000000 A4 : 0x00000000 A5 : 0x00000000
A6 : 0x00000000 A7 : 0x00000400 A8 : 0x00000000 A9 : 0x00000000
A10 : 0x3ffe3bd4 A11 : 0x00000000 A12 : 0x00000000 A13 : 0x00000000
A14 : 0x00ff0000 A15 : 0xff000000 SAR : 0x0000001b EXCCAUSE: 0x0000001c
EXCVADDR: 0x00000000 LBEG : 0x4008156e LEND : 0x40081580 LCOUNT : 0x00000000

Backtrace: 0x400f3090:0x3ffe3be0 0x400d14fc:0x3ffe3c00 0x400dc18f:0x3ffe3c20 0x40082f59:0x3ffe3c50 0x40079306:0x3ffe3c90 |<-CORRUPTED

Thanks a lot

RCmags commented 8 months ago

@mittlc Hi mittlc,

What output does your gyroscope have at rest? As mentioned in this issue and others, its critical the sensor reads (0,0,1) when held horizontally.

When you plot the velocity example, what do the graphs look like?

mittlc commented 8 months ago

Hi @RCmags, Thanks for the response and sorry for reopening both issues - i only saw later the more recent teensy issue from November and asked the same question there (and it was already answered). After chaning accIntegral.h i can now run the velocity example.

currently readings of the sensor when held still are 0.07, -0.01, 1.03

Do you want me to take this to the other issue - or better opening a new one for further discussion? i think you can close this one again.

RCmags commented 8 months ago

What line did you change in accIntegral.h?

I think there's an issue with the arduino IDE using code that's different from the one in this repo. It should be fixed now after making a new release.

I'll go ahead and close this one again.

RCmags / imuFilter

imuFilter with ESP32 #4

I changed this line vec3_t vel = 0 to vec3_t vel ={0,0,0}; and the example working now but the values are very bad and unstable

include // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050

include

define LP_FILTER 3 // Low pass filter. Value from 0 to 6

define GYRO_SENS 0 // Gyro sensitivity. Value from 0 to 3

define ACCEL_SENS 0 // Accelerometer sensitivity. Value from 0 to 3

define ADDRESS_A0 LOW // I2C address from state of A0 pin. A0 -> GND : ADDRESS_A0 = LOW

the code

include // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050

include

define LP_FILTER 3 // Low pass filter. Value from 0 to 6

define GYRO_SENS 0 // Gyro sensitivity. Value from 0 to 3

define ACCEL_SENS 0 // Accelerometer sensitivity. Value from 0 to 3

define ADDRESS_A0 LOW // I2C address from state of A0 pin. A0 -> GND : ADDRESS_A0 = LOW