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:
- 124:7d6b1b62483b
- Parent:
- 120:a0da2ce20a8e
- Child:
- 125:5b5a91004b48
--- a/run.cpp Fri Nov 19 07:56:16 2021 +0000 +++ b/run.cpp Fri Nov 19 09:21:51 2021 +0000 @@ -78,8 +78,8 @@ setDiag(Racc,50.0f); Matrix Rheading(2,2); - Rheading(1,1) = 0.001f; - Rheading(2,2) = 0.001f; + Rheading(1,1) = 0.1f; + Rheading(2,2) = 0.1f; //Rheading(3,3) = 0.001f; //Rheading(4,4) = 0.1f; _t.start(); @@ -115,12 +115,13 @@ }else{ //eskf.updateOtherConstraints(MatrixMath::Vector2mat(mag),palt,RotherConst); - eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst); + //eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst); //eskf.updateotherConstraints(MatrixMath::Vector2mat(mag),palt,RotherConst); //if(_t.read()>20){ //eskf.updateMag(MatrixMath::Vector2mat(mag),Rheading); //} - //eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc); + eskf.updateHeading(MatrixMath::Vector2mat(mag),Rheading); + eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc); } PIDtick.loop();