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:
- 99:98b892ca70ec
- Parent:
- 98:bdaa6bbfb1d9
- Child:
- 100:7589b663d462
diff -r bdaa6bbfb1d9 -r 98b892ca70ec run.cpp --- a/run.cpp Wed Nov 03 14:59:26 2021 +0000 +++ b/run.cpp Thu Nov 04 09:42:19 2021 +0000 @@ -8,6 +8,7 @@ if(hilFlag == false){ eskf.setQhat(rpy_align.x,rpy_align.y,rpy_align.z); } + int n_init = 1000; for(int i = 0;i<n_init;i++){ lsm.readAccel(); @@ -19,11 +20,13 @@ agoffset[3] +=(lsm.gx * M_PI / 180.0f); agoffset[4] +=(lsm.gy * M_PI / 180.0f); agoffset[5] +=(lsm.gz * M_PI / 180.0f); - palt0 = lps.pressureToAltitudeMeters(lps.readPressureMillibars()); + palt0 += lps.pressureToAltitudeMeters(lps.readPressureMillibars()); } for(int i = 0;i<6;i++){ agoffset[i] /= float(n_init); } + gps.Update(); + gps.CalculateUnit(); 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]); //センサ正常性チェック @@ -34,11 +37,11 @@ LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt); - //EKFの共分散設定 + //ESKFの共分散設定 eskf.setGravity(0.0f,0.0f,9.8f); eskf.setPhatPosition(0.1f); eskf.setPhatVelocity(0.1f); - eskf.setPhatAngleError(0.1f); + eskf.setPhatAngleError(10.0f); eskf.setPhatAccBias(0.001f); eskf.setPhatGyroBias(0.001f); eskf.setPhatGravity(0.0000001f); @@ -48,14 +51,16 @@ eskf.setQAccBias(0.0000001f); eskf.setQGyroBias(0.000001f); - Matrix Rgpspos(3,3); - setDiag(Rgpspos,0.9f); + Matrix Rgpspos(2,2); + setDiag(Rgpspos,2.0f); + //Rgpspos(3,3) = 5.0f; Matrix Rgpsvel(3,3); - setDiag(Rgpsvel,0.1f); + setDiag(Rgpsvel,2.0f); + Matrix RaccConst(4,4); setDiag(RaccConst,50.0f); - RaccConst(4,4) = 1.0f; + RaccConst(4,4) = 5.0f; //Matrix RgyroConst(2,2); //setDiag(RgyroConst,100); _t.start();