Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: run.cpp
- Revision:
- 122:68efdee114fa
- Parent:
- 120:a0da2ce20a8e
--- a/run.cpp Thu Nov 18 10:10:18 2021 +0000 +++ b/run.cpp Fri Nov 19 05:44:23 2021 +0000 @@ -49,12 +49,13 @@ eskf.setPhatAccBias(0.001f); eskf.setPhatGyroBias(0.001f); eskf.setPhatGravity(0.0000001f); + eskf.setPhatMagBias(0.00001f); eskf.setQVelocity(0.0025f); eskf.setQAngleError(0.00025f); eskf.setQAccBias(0.0000001f); eskf.setQGyroBias(0.000001f); - + eskf.setQMagBias(0.000001f); Matrix Rgps(5,5); setDiag(Rgps,2.0f); //Rgps(6,6) = 100.0f; @@ -66,10 +67,10 @@ setDiag(Rgpsvel,2.0f); //Rgpsvel(3,3) = 10.0f; - Matrix RImuConst(5,5); + Matrix RImuConst(4,4); setDiag(RImuConst,50.0f); - RImuConst(4,4) = 0.001f; - RImuConst(5,5) = 0.001f; + RImuConst(4,4) = 0.005f; + //RImuConst(5,5) = 0.5f; Matrix RotherConst(2,2); setDiag(RotherConst,0.1f); @@ -109,7 +110,7 @@ getGPSval(); } if(gpsUpdateFlag == true){ - eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps); + //eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps); //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),palt,Rgpspos); //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),Rgpsvel);