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:
- 104:20b8caa29185
- Parent:
- 103:fec71c2051c5
- Child:
- 106:2d854e92cebb
--- a/run.cpp Wed Nov 10 05:22:31 2021 +0000 +++ b/run.cpp Wed Nov 10 06:34:16 2021 +0000 @@ -64,28 +64,13 @@ eskf.setQGyroBias(0.000001f); eskf.setQMagField(0.001f); - Matrix Rgpspos(2,2); - setDiag(Rgpspos,2.0f); - //Rgpspos(3,3) = 5.0f; - Matrix Rgpsvel(3,3); - setDiag(Rgpsvel,2.0f); Matrix Rgps(5,5); setDiag(Rgps,2.0f); - - Matrix RaccConst(4,4); - setDiag(RaccConst,50.0f); - RaccConst(4,4) = 1.0f; - //Matrix RgyroConst(2,2); - //setDiag(RgyroConst,100); - Matrix Rmag(2,2); - Rmag(1,1) = 10.0f; - Rmag(2,2) = 1.0f; - Matrix RImuConst(5,5); setDiag(RImuConst,50.0f); - RImuConst(4,4) = 50.0f; - RImuConst(5,5) = 50.0f; + RImuConst(4,4) = 1.0f; + RImuConst(5,5) = 1.0f; _t.start(); float tgps = _t.read(); while(1) @@ -114,13 +99,11 @@ getGPSval(); } if(gpsUpdateFlag == true){ - //eskf.updateGPSPosition(MatrixMath::Vector2mat(pi),Rgpspos); - //eskf.updateGPSVelocity(MatrixMath::Vector2mat(vi),Rgpsvel); eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),Rgps); + }else{ + eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst); } - eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst); - //eskf.updateMag(MatrixMath::Vector2mat(mag),Rmag); PIDtick.loop(); //制御時間を計測