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:
- 111:0fae4fbe2a80
- Parent:
- 106:2d854e92cebb
- Child:
- 112:6a33ea9f6fed
--- a/run.cpp Fri Nov 12 12:56:48 2021 +0000 +++ b/run.cpp Mon Nov 15 13:42:01 2021 +0000 @@ -31,11 +31,6 @@ magref.x /= float(n_init); magref.y /= float(n_init); magref.z /= float(n_init); - if(hilFlag == false){ - eskf.setMagField(magref.x,magref.y,-magref.z); - }else{ - eskf.setMagField(magref.x,magref.y,magref.z); - } palt0 /= float(n_init); twelite.printf("Sensor offset : %f %f %f %f %f %f\r\n",agoffset[0],agoffset[1],agoffset[2],agoffset[3],agoffset[4],agoffset[5]); //センサ正常性チェック @@ -54,17 +49,19 @@ eskf.setPhatAccBias(0.001f); eskf.setPhatGyroBias(0.001f); eskf.setPhatGravity(0.0000001f); - eskf.setPhatMagField(0.00001f); + 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.setQMagField(0.001f); + eskf.setQMagBias(0.000001f); + eskf.setQMagRadius(0.000001f); - Matrix Rgps(6,6); + Matrix Rgps(5,5); setDiag(Rgps,2.0f); - Rgps(6,6) = 100.0f; + //Rgps(6,6) = 100.0f; Matrix Rgpspos(3,3); setDiag(Rgpspos,2.0f); @@ -74,8 +71,8 @@ //Rgpsvel(3,3) = 10.0f; Matrix RImuConst(5,5); - setDiag(RImuConst,100.0f); - RImuConst(4,4) = 10.0f; + setDiag(RImuConst,50.0f); + RImuConst(4,4) = 1.0f; RImuConst(5,5) = 1.0f; Matrix Rmag(3,3); @@ -109,12 +106,12 @@ getGPSval(); } if(gpsUpdateFlag == true){ - eskf.updateGPS(MatrixMath::Vector2mat(pi),palt,MatrixMath::Vector2mat(vi),MatrixMath::Vector2mat(mag),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); }else{ - //eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst); + eskf.updateImuConstraints(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(mag),RImuConst); //eskf.updateMag(MatrixMath::Vector2mat(mag),palt,Rmag); } PIDtick.loop();