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:
- 118:962f226bdf64
- Parent:
- 116:663f5889dd17
diff -r 663f5889dd17 -r 962f226bdf64 run.cpp --- a/run.cpp Wed Nov 17 01:40:40 2021 +0000 +++ b/run.cpp Thu Nov 18 00:50:47 2021 +0000 @@ -49,8 +49,8 @@ eskf.setPhatAccBias(0.001f); eskf.setPhatGyroBias(0.001f); eskf.setPhatGravity(0.0000001f); - eskf.setPhatGyroBias(0.01f); - eskf.setPhatMagRadius(0.0001f); + eskf.setPhatMagBias(0.001f); + eskf.setPhatMagRadius(0.001f); eskf.setQVelocity(0.0025f); eskf.setQAngleError(0.00025f); @@ -79,10 +79,14 @@ Matrix Racc(3,3); setDiag(Racc,50.0f); + Matrix Rother(2,2); + Rother(1,1) = 1.0f; + Rother(2,2) = 0.01f; + Matrix Rmag(3,3); - Rmag(1,1) = 0.01f; - Rmag(2,2) = 0.01f; - Rmag(3,3) = 0.01f; + Rmag(1,1) = 0.1f; + Rmag(2,2) = 0.1f; + Rmag(3,3) = 0.1f; //Rmag(4,4) = 0.01f; _t.start(); float tgps = _t.read(); @@ -116,9 +120,10 @@ //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),Rgpsvel); }else{ - //eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst); - eskf.updateMag(MatrixMath::Vector2mat(mag),Rmag); - eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc); + eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst); + //eskf.updateOtherConstraints(MatrixMath::Vector2mat(mag),palt,Rother); + //eskf.updateMag(MatrixMath::Vector2mat(mag),Rmag); + //eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc); } PIDtick.loop();