solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: run.cpp
- Revision:
- 94:579e875a4244
- Parent:
- 93:b827f78a717a
- Child:
- 96:3645f8f9bdd3
diff -r b827f78a717a -r 579e875a4244 run.cpp --- a/run.cpp Fri Oct 29 13:30:24 2021 +0000 +++ b/run.cpp Mon Nov 01 09:18:07 2021 +0000 @@ -32,21 +32,18 @@ LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt); - Timer _t; - _t.start(); - //EKFの共分散設定 eskf.setGravity(0.0f,0.0f,9.8f); eskf.setPhatPosition(0.1f); eskf.setPhatVelocity(0.1f); eskf.setPhatAngleError(0.1f); - eskf.setPhatAccBias(0.5f); - eskf.setPhatGyroBias(0.5f); - eskf.setPhatGravity(0.1f); + eskf.setPhatAccBias(0.001f); + eskf.setPhatGyroBias(0.001f); + eskf.setPhatGravity(0.0000001f); eskf.setQVelocity(0.0025f); eskf.setQAngleError(0.00025f); - eskf.setQAccBias(0.000001f); + eskf.setQAccBias(0.0000001f); eskf.setQGyroBias(0.000001f); Matrix Rgpspos(3,3); @@ -54,11 +51,12 @@ Matrix Rgpsvel(3,3); setDiag(Rgpsvel,0.1f); - Matrix RaccConst(3,3); - setDiag(RaccConst,400.0f); + Matrix RaccConst(4,4); + setDiag(RaccConst,50.0f); + RaccConst(4,4) = 1.0f; //Matrix RgyroConst(2,2); //setDiag(RgyroConst,100); - + _t.start(); float tgps = _t.read(); while(1) { @@ -82,7 +80,7 @@ eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel); tgps = _t.read(); }else{ - eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),RaccConst); + eskf.updateAccConstraints(MatrixMath::Vector2mat(acc),palt,RaccConst); //eskf.updateGyroConstraints(MatrixMath::Vector2mat(gyro),RgyroConst); } eskf.fuseErr2Nominal();