diff options
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 |