LSM303D Calibration and Tilt Compensated

Hi @jremington I have good news my boat is sailing straight :wink: !!!!!!!!!

I debugged my code again and found some bugs. Now on the compass lsm303d it flows straight;)

However, I have a question for you as an expert because I would like to fire it on the gy-91 sensor. But I'm stuck again.
I calibrated the magnetometer and accelerator. On the basis of calibrated data, it gives me the correct direction.
However, I have a problem with roll compensation.
I would like to know if I am doing it correctly. I don't know if I should also use a Low Pass Filterr? :

mySensor.accelUpdate();
  aX = mySensor.accelX();
  aY = mySensor.accelY();
  aZ = mySensor.accelZ();
  
  // Accelerometer calibration
  Xa_off = aX + 0.02;
  Ya_off = aY + 0.00;
  Za_off = aZ + 0.01;
  Xa_cal =  1013.19141*Xa_off - 8.52525*Ya_off + 10.43821*Za_off;
  Ya_cal = -8.52525*Xa_off + 1020.15433*Ya_off - 5.74794*Za_off;
  Za_cal = 10.43821*Xa_off + 0.09903*Ya_off + 970.49746*Za_off;




 //Low Pass Filter

   float fXg = Xa_cal * alpha + (fXg * (1.0 - alpha));
   float fYg = Ya_cal * alpha + (fYg * (1.0 - alpha));
   float fZg = Za_cal * alpha + (fZg * (1.0 - alpha));

 // pitch, roll

    pitch = atan2 (Ya_cal ,( sqrt ((Xa_cal * Xa_cal) + (Za_cal * Za_cal))));
    roll = atan2(-Xa_cal ,( sqrt((Ya_cal * Ya_cal) + (Za_cal * Za_cal))));


    // Tilt compensation
   float Yh = (Ym_cal * cos(roll)) - (Zm_cal * sin(roll));
   float Xh = (Xm_cal * cos(pitch))+(Ym_cal * sin(roll)*sin(pitch)) + (Zm_cal * cos(roll) * sin(pitch));




  float heading2=atan2(Xh, Yh);

  float declinationAngle2 = (5.0 + (41.0 / 60.0)) / (180 / M_PI);

  heading2 += declinationAngle2;

  
  
  if (heading2 < 0)
  {
    heading2 += 2 * PI;
  }
 
  if (heading2 > 2 * PI)
  {
    heading2 -= 2 * PI;
  }
 
  // Convert radians
  float headingDegreesWithTilt = heading2 * 180/M_PI; 

  Serial.println("With Tilt: " + String(headingDegreesWithTilt));

This code works for me without compensation, i.e. it correctly determines the degrees.

  float headingNoTilt = atan2(Xm_cal, Ym_cal);
 
 
  // Formula: (deg + (min / 60.0)) / (180 / M_PI);
  float declinationAngle = (4.0 + (26.0 / 60.0)) / (180 / M_PI);
  headingNoTilt += declinationAngle;
 
  // Korekta katow
  if (headingNoTilt < 0)
  {
    headingNoTilt += 2 * PI;
  }
 
  if (headingNoTilt > 2 * PI)
  {
    headingNoTilt -= 2 * PI;
  }
 
  // Radians convert
  float headingNoTiltDegrees = headingNoTilt * 180/M_PI;
 

  Serial.print(" Degress = ");
  Serial.print(headingNoTiltDegrees);
  Serial.println();

Thanks again for the time you gave me. For this joy, I forgot to make a movie by the water :slight_smile:
But maybe next week I will. Thank you in advance for your help

If the compass does not indicate approximately the same heading, regardless of how you tilt it, then the correction is wrong.

I prefer Pololu's vector method of tilt compensation, as it is so much easier to avoid programming errors.

Ok. I will try to analyze your code with tilt compensation :wink:
However, I have a question. Because I ran MPU-9250 .I used your calibration method. I got the following results:

Done. Gyro offsets (raw) 81.8, 82.0, 174.1
Collecting 300 points for scaling, 3/second
TURN SENSOR VERY SLOWLY AND CAREFULLY IN 3D
...
Done. rms Acc = 38550.11, rms Mag = 63.56

//Magnetometer:

301 measurements processed, expected 301

Expected norm of local field vector Hm? (Enter 0 for default     60.0) 41

Set Hm =     41.0

 float B[3]
 {  -17.69,   42.52,  -24.24};

 float Ainv[3][3]
  {{  0.98847,  0.00219, -0.01255},
  {  0.00219,  0.98067,  0.06008},
  { -0.01255,  0.06008,  0.99690}};
  
 // ACC


301 measurements processed, expected 301

Expected norm of local field vector Hm? (Enter 0 for default  38260.8) 1000

Set Hm =   1000.0

 float B[3]
 {-33032.12,-16970.91,-3181.73};

 float Ainv[3][3]
  {{  0.09860,  0.00091,  0.00009},
  {  0.00091,  0.07507, -0.00032},
  {  0.00009, -0.00032,  0.06433}};
  

And now I have to enter the following values into your code


float A_cal[6] = {515.0, 279.0, 751.0, 5.96e-5, 6.26e-5, 6.06e-5}; // 0..2 offset xyz, 3..5 scale xyz

float M_cal[6] = {17.69, -42.52, -24.24, 0.01403, 0.01414, 0.01387}; // can make both 3x3 to handle off-diagonal corrections

But apart from the offset, I don't know which values to enter ... under A1, A2, A3 and M1, M3, M3


float A_cal[6] = {33032.12, 16970.9, 3181.73, A1 , A2,  A3}; // 0..2 offset xyz, 3..5 scale xyz

float M_cal[6] = {17.69, -42.52, 24.24, M1, M2, M3}; // can make both 3x3 to handle off-diagonal corrections

Could you please tell me how to transfer this matrix to your code ?? Thank you in advance.

Those are the offsets and the diagonal elements of the calibration matrix.

In your case, for the magnetometer, enter the following (DO NOT CHANGE SIGNS):

float M_cal[6] = {-17.69, 42.52, -24.24, 0.98847, 0.98067, 0.99690};

Ok, I changed how you wrote, but this thing does not work for me.

float A_cal[6] = {-33032.12,-16970.91,-3181.73, 0.09860, 0.07507, 0.06433}; 
float M_cal[6] = {-17.69, 42.52, -24.24, 0.98847, 0.98067, 0.99690};

I don't know, but the acceleration readings seem strange to me:
This is the raw reading of:

-32768 -16528 13168
-32768 -14836 12756
-32768 -14684 12460
-32768 -16144 12780
-32768 -17144 13620
-32768 -19492 13192
-32768 -17936 12824
-32768 -16512 13164
-32768 -18536 11860
-32768 -19328 13364
-32768 -18228 14204
-32768 -19640 13172
-32768 -19028 11912
-32768 -19884 12952
-32768 -18228 13360
-32768 -19104 12744
-32768 -20216 12940
-32768 -18712 12740
-32768 -17868 12832
-32768 -18540 12968
-32768 -19404 13092
-32768 -17740 13008
-32768 -17664 12980
-32768 -17276 12784
-32768 -16944 13260
-32768 -17028 13680
-32768 -17948 12520
-32768 -17712 12820
-32768 -19024 13016
-32768 -17380 12644
-32768 -17244 13140
-32768 -16448 12900
-32768 -16972 14336
-32768 -15780 11900
-32768 -16376 12656
-32768 -15296 12624
-32768 -16136 13484
-32768 -14960 11896
-32768 -16404 12760
-32768 -17744 12332
-32768 -17652 12556
-32768 -17380 12696
-32768 -15224 13260
-32768 -13456 11868
-32768 -10752 11456
-32768 -8780 10408
-32768 -7496 8952
-32768 -4904 6788
-32768 -3704 4948
-32768 -3060 3452
-32768 -2988 2732
-32768 -2948 168
-32768 -3172 -1272
-32768 -4180 -1412
-32768 -5616 -2328
-32768 -7192 -2760
-32768 -8776 -2700
-32768 -11224 -2844
-32768 -12420 -3608
-32768 -13632 -4772
-32768 -14368 -3784
-32768 -14772 -6096
-32768 -15372 -7304
-32768 -17040 -8416
-32768 -19088 -11412
-32768 -21776 -11684
-32768 -23244 -13224
-32768 -24780 -14780
-32768 -27020 -16768
-32768 -28768 -12844
-32768 -29884 -13232
-32768 -30300 -13136
-32768 -31908 -12784
-32768 -31920 -11520
-32768 -32768 -9984
-32768 -32768 -7880
-32768 -32768 -5504
-31268 -32768 -3168
-29736 -29940 -272
-27660 -29424 2316
-27220 -26592 2088
-25820 -26764 2664
-25528 -23404 4736
-25860 -21396 4160
-24756 -18944 4956
-24180 -17388 4184
-24632 -14632 4496
-25380 -13188 3652
-25720 -10848 2852
-27568 -8220 3436
-28180 -6492 3236
-27752 -6104 -144
-31104 -5796 4476
-30556 -4988 2984
-30768 -4376 2168
-31656 -4516 2516
-32740 -4396 4068
-32768 -3944 2816
-32768 -3028 2384
-32768 -2324 2984
-32768 -984 1912
-32768 -2740 1616
-32768 -3432 1572
-32768 -4528 1132
-32768 -6092 2264
-32768 -9028 1440
-32768 -11872 1524
-32768 -14740 2552
-32768 -17080 2584
-32768 -18576 3704
-32768 -20132 2440
-32768 -22604 1008
-32768 -23164 2832
-32768 -23692 2712
-32768 -23836 5128
-32768 -23560 7604
-32768 -23888 9916
-32768 -24736 11544
-32768 -25856 10980
-32768 -26128 9744
-32768 -27288 9180
-31968 -28144 7848
-30016 -28772 6092
-28328 -28256 3224
-27960 -28388 1400
-26696 -28712 -632
-27652 -28364 -1448
-27124 -27944 -3192
-25956 -26960 -7488
-26116 -26432 -9028
-26184 -24916 -10764
-26108 -23932 -12484
-26888 -21540 -14228
-28388 -18760 -16464
-29644 -16684 -15796
-30724 -12588 -13232
-31152 -11984 -16984
-32676 -9936 -17404
-32768 -9376 -17760
-32768 -8984 -17072
-32768 -8768 -17188
-32768 -9736 -17016
-32768 -10532 -16064
-32768 -10572 -14180
-32768 -12476 -12516
-32768 -13708 -12180
-32768 -15604 -8292
-32768 -16848 -5240
-32768 -17416 -3068
-32768 -17328 -596
-32768 -18412 2608
-32768 -18104 4276
-32768 -18808 6436
-32768 -18260 6620
-32768 -19936 8300
-32768 -20124 10208
-32768 -19600 12196
-32768 -19504 12868
-32768 -17796 12876
-32768 -17740 12420
-31548 -18140 10728
-32768 -19692 9296
-31972 -19172 11316
-29520 -19384 8028
-26196 -18280 6312
-23636 -17988 2316
-22160 -17996 -5488
-23340 -18664 -10600
-27128 -18000 -14688
-29316 -17696 -16548
-32096 -17576 -18044
-32768 -18696 -18460
-32768 -19308 -19892
-32768 -18040 -19896
-32768 -20392 -19640
-32768 -22460 -14368
-32768 -23156 -12676
-32768 -24012 -8580
-32768 -24788 -3968
-32768 -24528 228
-32768 -25116 2592
-32768 -25316 4104
-32768 -25976 5324
-32768 -26532 8280
-32768 -24348 10024
-32768 -24440 10760
-32768 -24508 11576
-32768 -24068 10868
-32768 -22636 12616
-32768 -22020 11400
-32768 -22504 12556
-32768 -23504 11200
-32768 -24220 14104
-32768 -26076 11128
-32768 -27320 9692
-32768 -28076 9904
-32768 -29060 7884
-32768 -29064 8564
-32768 -28400 8180
-32768 -27628 8328
-32768 -27128 9400
-32768 -23576 10428
-32768 -21524 11740
-32768 -18676 12304
-32768 -15860 12156
-32768 -13240 11988
-32768 -10508 10744
-32768 -8992 8764
-30372 -7648 5524
-30252 -6880 5268
-27844 -6444 -128
-27876 -5284 -1784
-28504 -6064 -4064
-27664 -5672 -5216
-28320 -6488 -8256
-28488 -6368 -9032
-28772 -7604 -11856
-29932 -8512 -12280
-30348 -10120 -15168
-31844 -10996 -15396
-32768 -13048 -18828
-32768 -14980 -17988
-32768 -15860 -18848
-32768 -16772 -20244
-32768 -16240 -20360
-32768 -16956 -20784
-32768 -17576 -20644
-32768 -18388 -19304
-32768 -18224 -22252
-32768 -17152 -20112
-32768 -18312 -19748
-32768 -19448 -19048
-32768 -18380 -19356
-32768 -22084 -18300
-32768 -20268 -20012
-32768 -21388 -18792
-32768 -20632 -20112
-32768 -20372 -20504
-32768 -20536 -19060
-32768 -19908 -19508
-32768 -18952 -20380
-32768 -18140 -20468
-32768 -17632 -22008
-32768 -18520 -20496
-32768 -17896 -19340
-32768 -17280 -19228
-32768 -17808 -18696
-32768 -19184 -19096
-32768 -18300 -19100
-32768 -18020 -19408
-32768 -14452 -21060
-32768 -12172 -18648
-32768 -10696 -19100
-32768 -8464 -17960
-32768 -6360 -16484
-32768 -4988 -14388
-32768 -2108 -11120
-32768 -1060 -5572
-32768 -816 -3052
-32768 -2660 -2064
-32768 -1308 1224
-32768 -3132 2680
-32768 -2872 3292
-32768 -2896 3736
-32768 -4632 4692
-32768 -5120 6264
-32768 -4504 7412
-32768 -6068 7308
-32768 -7580 6952
-32768 -6176 7612
-32768 -7240 9196
-32768 -9228 8940
-32768 -11648 11824
-32768 -12228 10636
-32768 -14144 12520
-32768 -16108 13092
-32768 -17200 11304
-32768 -18044 13528
-32768 -20372 12488
-32768 -20580 12976
-32768 -22212 13364
-32768 -21848 11752
-32768 -21648 12592
-32768 -21608 9316
-32768 -21060 13056
-32768 -21392 12588
-32768 -16848 14832
-32768 -15260 11652
-32768 -13272 10912
-32768 -12580 11068
-32768 -13208 10436
-32768 -11716 10068
-32768 -10640 10016
-32768 -8472 7936
-32768 -7308 5196
-32768 -5840 4496
-32768 -3008 2312
-32768 -1800 940
-32768 -1548 -2260
-32768 -1032 -4636
-32768 -1612 -5128

Corrected values

Set Hm =   1000.0

 float B[3]
 {-33032.12,-16970.91,-3181.73};

 float Ainv[3][3]
  {{  0.09860,  0.00091,  0.00009},
  {  0.00091,  0.07507, -0.00032},
  {  0.00009, -0.00032,  0.06433}};

[tiltcompensate] I slowly move the compass on the ground from north to south clockwise

69 <-- North
69
70
67
69
69
69
69
69
69
69
70
69
69
68
69
70
70
68
70
69
69
70
69
69 <---South

And I should be between 0-180 degrees here but I have some strange values; (

The calibration corrections are clearly wrong.

As already mentioned, to test whether the code to apply the calibration values is working properly, print out the corrected sensor values and run them through magneto again.

You should get offsets of about zero, and diagonal matrix elements of about 1.000

Specifically for the MPU9250, I chose to not implement the off-diagonal matrix elements because they did not make any difference. And, that sensor was discontinued several years ago, and is not longer manufactured.

You can change the code to include those matrix elements, it is in several other of my AHRS repositories.