diff options
| author | Miller | 2017-07-04 13:35:51 -0500 |
|---|---|---|
| committer | Miller | 2017-07-04 13:35:51 -0500 |
| commit | d2070eef13177191a25e97cdb78b60bd58743a9e (patch) | |
| tree | cee668fc5603ab5f177d1479984226dfe517ca1f /src/body.cpp | |
| parent | 39a1546198d23e6debbf5dab1c123ea377f677bc (diff) | |
| download | refslam-d2070eef13177191a25e97cdb78b60bd58743a9e.zip refslam-d2070eef13177191a25e97cdb78b60bd58743a9e.tar.gz | |
params for body noise
Diffstat (limited to 'src/body.cpp')
| -rw-r--r-- | src/body.cpp | 10 |
1 files changed, 5 insertions, 5 deletions
diff --git a/src/body.cpp b/src/body.cpp index 018075b..5579e4d 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -101,7 +101,7 @@ Matrix<double,1,1> Body::R() { Matrix<double,1,1> R; - R << R_HEIGHT; + R << r_height; return R; } @@ -142,12 +142,12 @@ Body::Q (double dt) #endif Matrix<double,STATESIZE,STATESIZE> Q; Q = Matrix<double,STATESIZE,STATESIZE>::Zero(); - Q = ACC_STD*ACC_STD*Ga*Ga.transpose(); - Q += ACC_BIAS_STD*ACC_BIAS_STD*Gb*Gb.transpose(); - Q += ANG_STD*ANG_STD * Gw * Gw.transpose(); + Q = acc_std*acc_std*Ga*Ga.transpose(); + Q += acc_bias_std*acc_bias_std*Gb*Gb.transpose(); + Q += ang_std*ang_std * Gw * Gw.transpose(); #if STATESIZE==13 - Q += ANG_STD*ANG_STD *Gq * Gq.transpose(); + Q += ang_std*ang_std *Gq * Gq.transpose(); #endif |