Closed MohamedGaberZidan closed 8 months ago
Hey, I'll first focus on your issue with vector_datatype. We can work up from there to make all the libraries work.
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.
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
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.
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
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
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.
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] */
// ========== IMU sensor ==========
// Gyro settings:
// 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.
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.
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
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.
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
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.
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.
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.
There's two ways to scale the velocity:
It comes from calculus. A constant can exist inside or outside the integration sign:
It might be the bug you're experiencing is related to the acceleration being scaled.
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.
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.
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.
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?
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;
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.
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.
Umm, if the position barely changes with velocity, how exactly did it get to 26?
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 :(.
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.
Here is it, first 2 seconds
moving the IMU
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?
Here is it sorry,
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.
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.
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.
Actually, It didn't go well at all.
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.
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
/ 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] */
// ========== IMU sensor ==========
// Gyro settings:
// 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
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;
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
@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?
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.
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.
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