solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
run.cpp
- Committer:
- NaotoMorita
- Date:
- 2021-11-19
- Revision:
- 124:7d6b1b62483b
- Parent:
- 120:a0da2ce20a8e
- Child:
- 125:5b5a91004b48
File content as of revision 124:7d6b1b62483b:
#include "global.hpp" void run() { wait(0.5); //センサの初期化・ジャイロバイアス・加速度スケールの取得 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(); lsm.readMag(); lsm.readGyro(); agoffset[0] += lsm.ax * 9.8f; agoffset[1] += lsm.ay * 9.8f; agoffset[2] += lsm.az * 9.8f-9.8f; 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()); magref.x += lsm.mx; magref.y += lsm.my; magref.z += lsm.mz; } for(int i = 0;i<6;i++){ agoffset[i] /= float(n_init); } magref.x /= float(n_init); magref.y /= float(n_init); magref.z /= float(n_init); 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]); //センサ正常性チェック //usaPack通信開始 pc.Subscribe(0000, &(vp)); //制御ループのアタッチ LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt); //ESKFの共分散設定 eskf.setGravity(0.0f,0.0f,9.8f); eskf.setPhatPosition(0.1f); eskf.setPhatVelocity(0.1f); eskf.setPhatAngleError(0.5f); eskf.setPhatAccBias(0.001f); eskf.setPhatGyroBias(0.001f); eskf.setPhatGravity(0.0000001f); eskf.setQVelocity(0.0025f); eskf.setQAngleError(0.00025f); eskf.setQAccBias(0.0000001f); eskf.setQGyroBias(0.000001f); Matrix Rgps(5,5); setDiag(Rgps,2.0f); //Rgps(6,6) = 100.0f; Matrix Rgpspos(3,3); setDiag(Rgpspos,2.0f); Matrix Rgpsvel(3,3); setDiag(Rgpsvel,2.0f); //Rgpsvel(3,3) = 10.0f; Matrix RImuConst(5,5); setDiag(RImuConst,50.0f); RImuConst(4,4) = 0.001f; RImuConst(5,5) = 0.001f; Matrix RotherConst(2,2); setDiag(RotherConst,0.1f); Matrix Racc(3,3); setDiag(Racc,50.0f); Matrix Rheading(2,2); Rheading(1,1) = 0.1f; Rheading(2,2) = 0.1f; //Rheading(3,3) = 0.001f; //Rheading(4,4) = 0.1f; _t.start(); float tgps = _t.read(); while(1) { float tstart = _t.read(); //センサの値を取得 if(hilFlag == true){ getHilIMUval(); }else{ getIMUval(); } //ekfの更新 eskf.updateNominal(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt); eskf.updateErrState(MatrixMath::Vector2mat(acc),MatrixMath::Vector2mat(gyro),att_dt); if(hilFlag == true){ if(tstart-tgps>0.2f){ getHilGPSval(); tgps = _t.read(); gpsUpdateFlag = true; }else{ gpsUpdateFlag = false; } }else{ getGPSval(); } if(gpsUpdateFlag == true){ 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.updateOtherConstraints(MatrixMath::Vector2mat(mag),palt,RotherConst); //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.updateHeading(MatrixMath::Vector2mat(mag),Rheading); eskf.updateAcc(MatrixMath::Vector2mat(acc),Racc); } PIDtick.loop(); //制御時間を計測 float tend = _t.read(); att_dt = (tend-tstart); } }