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:
- 113:3e47d9881529
- Parent:
- 112:6a33ea9f6fed
- Child:
- 114:88abd741f44d
- Child:
- 117:f899fd694e2a
diff -r 6a33ea9f6fed -r 3e47d9881529 run.cpp --- a/run.cpp Tue Nov 16 01:21:01 2021 +0000 +++ b/run.cpp Tue Nov 16 13:57:32 2021 +0000 @@ -45,19 +45,15 @@ eskf.setGravity(0.0f,0.0f,9.8f); eskf.setPhatPosition(0.1f); eskf.setPhatVelocity(0.1f); - eskf.setPhatAngleError(0.1f); + eskf.setPhatAngleError(0.5f); eskf.setPhatAccBias(0.001f); eskf.setPhatGyroBias(0.001f); eskf.setPhatGravity(0.0000001f); - eskf.setPhatMagBias(0.001f); - eskf.setPhatMagRadius(0.5f); eskf.setQVelocity(0.0025f); eskf.setQAngleError(0.00025f); eskf.setQAccBias(0.0000001f); eskf.setQGyroBias(0.000001f); - eskf.setQMagBias(0.000001f); - eskf.setQMagRadius(0.000001f); Matrix Rgps(5,5); setDiag(Rgps,2.0f); @@ -75,10 +71,15 @@ RImuConst(4,4) = 0.01f; RImuConst(5,5) = 0.01f; + + Matrix Racc(3,3); + setDiag(Racc,50.0f); + Matrix Rmag(3,3); - Rmag(1,1) = 10.0f; - Rmag(2,2) = 1.0f; - Rmag(3,3) = 2.0f; + Rmag(1,1) = 0.1f; + Rmag(2,2) = 0.001f; + Rmag(3,3) = 0.1f; + //Rmag(4,4) = 0.001f; _t.start(); float tgps = _t.read(); while(1) @@ -111,8 +112,9 @@ //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),Rgpsvel); }else{ - eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst); - //eskf.updateMag(MatrixMath::Vector2mat(mag),palt,Rmag); + //eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst); + eskf.updateMag(MatrixMath::Vector2mat(mag),Rmag); + eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc); } PIDtick.loop();