Small pertubations cause large errors

[Peter Milani]

Hi All,

This is a user question. My problem is a unit test replicating  two gps measurements with a linear acceleration in x direction, and no angular velocity, and a MagPose Factor based on a magnetic measurement integrated between the two GPS measurements. 

There is a prior on the pose at the start which I make inaccurate with a small 0.2 radian error in Yaw. However this results in a large error on my final state.

As you can see there is only very modest errors in gps measurement (less than 1 sigma) and my magnetic aligment should be perfect (noiseless).

This is my factor graph:

NonlinearFactorGraph: size: 10

Factor 0:  GPSFactor on x0
  GPS measurement:   4
  0
-10
  noise model: diagonal sigmas [5.22000006; 5.22000006; 10];

Factor 1: PriorFactor on x0
  prior mean:  R: [
0.980066578, -0.198669331, 0;
0.198669331, 0.980066578, 0;
0, 0, 1
]
t: 0 0 0
isotropic dim=6 sigma=1000

Factor 2: PriorFactor on v0
  prior mean: [
0;
0;
0
]
  noise model: unit (3)

Factor 3: PriorFactor on b0
  prior mean: acc = 0 0 0 gyro = 0 0 0
  noise model: unit (6)

Factor 4:
ImuFactor(x0,v0,x1,v1,b0)
preintegrated measurements:

    deltaTij = 0.51
    deltaRij.ypr = ( 0 -0  0)
    deltaPij =   0.013005          0 -1.2757905
    deltaVij =   0.051       0 -5.0031
    gyrobias = 0 0 0
    acc_bias = 0 0 0

    preintMeasCov
[        5.1e-05               0               0               0  2.10547125e-05               0               0    0.0001250775               0
              0         5.1e-05               0 -2.10547125e-05               0    -2.14625e-07   -0.0001250775               0      -1.275e-06
              0               0         5.1e-05               0     2.14625e-07               0               0       1.275e-06               0
              0 -2.10547125e-05               0   0.00178431386               0  1.61047496e-07   0.00528022191               0  7.97369063e-07
 2.10547125e-05               0     2.14625e-07               0    0.0017843155               0               0   0.00528023003               0
              0    -2.14625e-07               0  1.61047496e-07               0   0.00176851674  7.97369063e-07               0   0.00520200813
              0   -0.0001250775               0   0.00528022191               0  7.97369063e-07    0.0208130935               0   4.2109425e-06
   0.0001250775               0       1.275e-06               0   0.00528023003               0               0    0.0208131364               0
              0      -1.275e-06               0  7.97369063e-07               0   0.00520200813   4.2109425e-06               0    0.0204000429]
  noise model sigmas: 0.00714142843 0.00714142843 0.00714142843  0.0422411394  0.0422411589  0.0420537364   0.144267437   0.144267586   0.142828719

Factor 5:   keys = { x1 }
isotropic dim=3 sigma=0.3
local field (nM): [278.188311; 56.2520937; -446.972678];
measured field (bM): [278.1884; 56.2520496; -446.972628];
magnetometer bias: [0; 0; 0];

Factor 6: BetweenFactor(b0,b1)
  measured: acc = 0 0 0 gyro = 0 0 0
isotropic dim=6 sigma=0.0714142843

Factor 7:
ImuFactor(x1,v1,x2,v2,b1)
preintegrated measurements:

    deltaTij = 99.49
    deltaRij.ypr = (-2.91647641e-06   2.1446921e-09 -4.59183659e-06)
    deltaPij =     494.91297 -0.0747825583   -48550.9658
    deltaVij =     9.94899895 -0.00225509046      -975.9969
    gyrobias =    4.615375e-08 -2.16241642e-11  2.93142668e-08
    acc_bias = 1.27141331e-13 2.07411884e-14 4.14718229e-13

    preintMeasCov
[       0.009949  8.18879196e-18 -1.11030698e-14  0.000234757264      160.986912 -0.000369594103  7.07917568e-06      4.85460858 -1.48843488e-05
 8.18879203e-18        0.009949  5.20338481e-18     -160.986912   0.00023098955     -1.64104887     -4.85460858  6.96555914e-06    -0.049486319
-1.11030698e-14  5.20338501e-18        0.009949  2.37434786e-06      1.64104905 -3.76771453e-06  3.81072699e-06    0.0494863242 -1.13616561e-07
 0.000234757264     -160.986912  2.37434786e-06      4702301.01    0.0981774678      47799.9001      118034.088   0.00259049952      1201.18356
     160.986912   0.00023098955      1.64104905    0.0981774678      4702788.27     -9.63121053   0.00333064232      118046.332    -0.326736045
-0.000369594103     -1.64104887 -3.76771453e-06      47799.9001     -9.63121053      13617.6288       1201.1836    -0.254128036      210.209682
 7.07917568e-06     -4.85460858  3.81072699e-06      118034.088   0.00333064232       1201.1836       3162.5428  9.29967726e-05      32.1973781
     4.85460858  6.96555914e-06    0.0494863242   0.00259049952      118046.332    -0.254128036  9.29967726e-05      3162.87101  -0.00912298462
-1.48843488e-05    -0.049486319 -1.13616561e-07      1201.18356    -0.326736045      210.209682      32.1973781  -0.00912298462      4.30780975]
  noise model sigmas: 0.099744674 0.099744674 0.099744674  2168.47896  2168.59131  116.694596    56.23649   56.239408  2.07552638

Factor 8:  GPSFactor on x2
  GPS measurement: 504
  0
-10
  noise model: diagonal sigmas [5.22000006; 5.22000006; 10];

Factor 9: BetweenFactor(b1,b2)
  measured: acc = 0 0 0 gyro = 0 0 0
isotropic dim=6 sigma=0.99744674

I optimise midway at key=1 and that seems okay, but the small perturbations in position play havoc on the values estimation (based on a preintegratedimu) for the next optimisation. I am resetting the preintegratedImu with new bias which are small.

Any suggestiions, and appreciate the help.

[Peter Milani]

I should have note that my optimised enstate is  Position3 (835.29, 78.19, 149.46) which should have been close to (500, 0, -10);

cheers

Peter