solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: main.cpp
- Revision:
- 44:f571273d3223
- Parent:
- 43:f0dd73430192
- Child:
- 45:b439a135c24b
- Child:
- 46:99ffb98e1257
--- a/main.cpp Fri Apr 02 07:22:09 2021 +0000 +++ b/main.cpp Thu Apr 08 07:37:23 2021 +0000 @@ -43,6 +43,7 @@ Timer t; Matrix qhat(4,1); +Matrix qhat_gyro(4,1); Matrix Phat(4,4); Matrix Qgyro(3,3); Matrix Racc(3,3); @@ -56,6 +57,11 @@ float pitch = 0.0; float roll = 0.0; float yaw = 0.0; + +float pitch_g = 0.0; +float roll_g = 0.0; +float yaw_g = 0.0; + int16_t ax, ay, az; int16_t gx, gy, gz; MotionSensorDataUnits mdata; @@ -93,21 +99,22 @@ float accref[3] = {0, 0, -0.98}; float magref[3] = {0.65, 0, 0.75}; -int agoffset[6] = {0, 0, 0, -123, -575, -1}; -float magbias[4] = {-143.593872, 125.311264, -161.226898, 27.998077}; +int agoffset[6] = {0, 0, 0, -117, -563, 2 }; +float magbias[4] = {-140.868240, 121.863251, -162.735092, 37.112610}; void writeSdcard() { //magcal.getExtremes(&magmin[0],&magmax[0]); //pc.printf("%f %f %f %f %f %f\r\n",magmin[0],magmin[1],magmin[2],magmax[0],magmax[1],magmax[2]); //pc.printf("%f %f %f %f %f %f %f %f %f %f %f %f \r\n",acc_x,acc_y,acc_z,mag_x,mag_y,mag_z,accref[0],accref[1],accref[2],magref[0],magref[1],magref[2]); - //pc.printf("%f %d %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,obs_count,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); - pc.printf("%f %f %f %f : %f %f %f\r\n",float(obs_count)/100.0,th_mg,dynaccnorm,accnormerr,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); + sd.printf("%f %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n",1.0/att_dt,obs_count,acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,mag_x,mag_y,mag_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi); + //pc.printf("%f %f %f %f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi,roll_g*180.0f/pi,pitch_g*180.0f/pi,yaw_g*180.0f/pi); + //pc.printf("%f %f %f %f : %f %f %f\r\n",float(obs_count)/100.0,th_mg,dynaccnorm,accnormerr,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); //pc.printf("%d \r\n",userButton.read()); //pc.printf("%f %f %f %f %f %f\r\n",gyro_x*180.0f/pi,gyro_y*180.0f/pi,gyro_z*180.0f/pi,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); //pc.printf("%f %f %f\r\n",roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); //pc.printf("%f %f %f %f %f %f\r\n",dynacc_x,dynacc_y,dynacc_z,roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); - //pc.printf("%f %f %f %f %f : %f %f %f\r\n",f,magbias[0],magbias[1],magbias[2],magbias[3],roll*180.0f/pi,pitch*180.0f/pi,yaw*180.0f/pi); + //pc.printf("%f %f %f : %f %f %f\r\n",magref[0],magref[1],magref[2],mag_x,mag_y,mag_z); //pc.printf("%f %f %f %f %f : %f %f %f %f %f %f\r\n",f,magbias[0],magbias[1],magbias[2],magbias[3],magval[0],magval[1],magval[2],mdata.x,mdata.y,mdata.z); } @@ -117,17 +124,13 @@ } void calcMagRef(float mx, float my, float mz){ - float q0 = qhat.getNumber( 1, 1 ); - float q1 = qhat.getNumber( 2, 1 ); - float q2 = qhat.getNumber( 3, 1 ); - float q3 = qhat.getNumber( 4, 1 ); - float hx = 2.0f * (mx * (0.5f - q2*q2 - q3*q3) + my * (q1*q2 - q0*q3) + mz * (q1*q3 + q0*q2)); - float hy = 2.0f * (mx * (q1*q2 + q0*q3) + my * (0.5f - q1*q1 - q3*q3) + mz * (q2*q3 - q0*q1)); - float bx = sqrt(hx * hx + hy * hy); - float bz = 2.0f * (mx * (q1*q3 - q0*q2) + my * (q2*q3 + q0*q1) + mz * (0.5f - q1*q1 - q2*q2)); - magref[0] = bx; + + Matrix magvec(3,1); + magvec << mx << my << mz; + Matrix magnedvec = MatrixMath::Transpose(D)*magvec; + magref[0] = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1)); magref[1] = 0.0; - magref[2] = bz; + magref[2] = magnedvec(3,1); } @@ -195,11 +198,15 @@ << gyro_x*0.5*att_dt << 1.0 << gyro_z*0.5*att_dt <<-gyro_y*0.5*att_dt << gyro_y*0.5*att_dt << -gyro_z*0.5*att_dt << 1.0 << gyro_x*0.5*att_dt << gyro_z*0.5*att_dt << gyro_y*0.5*att_dt <<-gyro_x*0.5*att_dt << 1.0; + qhat = phi*qhat; - float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); qhat *= (1.0f/ qnorm); + qhat_gyro = phi*qhat_gyro; + qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat_gyro),qhat_gyro)); + qhat_gyro *= (1.0f/ qnorm); + Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B); q0 = qhat.getNumber( 1, 1 ); @@ -295,6 +302,15 @@ roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align; pitch = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align; yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); + + q0 = qhat_gyro.getNumber( 1, 1 ); + q1 = qhat_gyro.getNumber( 2, 1 ); + q2 = qhat_gyro.getNumber( 3, 1 ); + q3 = qhat_gyro.getNumber( 4, 1 ); + roll_g = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-roll_align; + pitch_g = asinf(-2.0f * (q1*q3 - q0*q2))-pitch_align; + yaw_g = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); + } void triad(float fb1,float fb2, float fb3, float fn1, float fn2, float fn3,float mb1,float mb2, float mb3, float mn1, float mn2, float mn3){ @@ -342,6 +358,24 @@ float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); qhat *= (1.0f/ qnorm); + + qhat_gyro = qhat; + + float q0 = qhat.getNumber( 1, 1 ); + float q1 = qhat.getNumber( 2, 1 ); + float q2 = qhat.getNumber( 3, 1 ); + float q3 = qhat.getNumber( 4, 1 ); + + D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3); + D.add(1,2,2*(q1*q2 + q0*q3)); + D.add(1,3,2*(q1*q3 - q0*q2)); + D.add(2,1,2*(q1*q2 - q0*q3)); + D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3); + D.add(2,3,2*(q2*q3 + q0*q1)); + D.add(3,1,2*(q1*q3 + q0*q2)); + D.add(3,2,2*(q2*q3 - q0*q1)); + D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3); + } void calcDynAcc(){ @@ -472,13 +506,35 @@ servoLeft.pulsewidth_us(servoOut[1]); servoThrust.pulsewidth_us(motorOut[0]); - if(loop_count > 10){ + //観測アップデート + calcDynAcc(); + th_mg = abs(acos((LPmag_x*LPacc_x+LPmag_y*LPacc_y+LPmag_z*LPacc_z)/LPmagnorm/LPaccnorm)-val_thmg); + dynaccnorm = sqrt(dynacc_x*dynacc_x+dynacc_y*dynacc_y+dynacc_z*dynacc_z); + accnormerr = abs(LPaccnorm-accrefnorm); + //静止時100個の平均 0.01877522 0.00514146 0.00477393 + + //int ang_th = th_mg < 0.01877522; + //int dyn_th = dynaccnorm < 0.00514146; + //int norm_th = accnormerr< 0.00477393; + int ang_th = th_mg < 0.01877522/5.0; + int dyn_th = dynaccnorm < 0.00514146/5.0; + int norm_th = accnormerr< 0.00477393/5.0; + if(dyn_th+ang_th+norm_th>0){ + //if(dyn_th+ang_th+norm_th>-1){ + obs_count += 1; + updateAcrossMeasures(LPmag_x/LPmagnorm,LPmag_y/LPmagnorm,LPmag_z/LPmagnorm,magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag); + updateAcrossMeasures(LPacc_x/LPaccnorm,LPacc_y/LPaccnorm,LPacc_z/LPaccnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc); + } + + if(loop_count >= 10){ writeSdcard(); loop_count = 1; obs_count = 0; }else{ loop_count +=1; } + + } int main() @@ -522,23 +578,28 @@ if(userButton.read() == 0){ qhat << 1 << 0 << 0 << 0; + + D.add(1,1,1.0); + D.add(2,2,1.0); + D.add(3,3,1.0); Phat << 0.01 << 0 <<0 <<0 << 0 << 0.01 <<0 <<0 << 0 << 0 <<0.001 <<0 << 0 << 0 << 0 << 0.001; - Qgyro << 5.4732e-04 << 0 <<0 - << 0 <<5.4732e-04 <<0 - << 0 << 0 <<5.4732e-04; + Qgyro << 0.0224<< 0 <<0 + << 0 <<0.0224<<0 + << 0 << 0 <<0.0224; - Racc.add(1,1,1.0); - Racc.add(2,2,1.0); - Racc.add(3,3,1.0); - - Rmag << 1 << 0 <<0 - << 0 << 1 <<0 - << 0 << 0 <<1; + Racc.add(1,1,0.0330*100); + Racc.add(2,2,0.0330*100); + Racc.add(3,3,0.0330*100); + + Rmag.add(1,1,1.0); + Rmag.add(2,2,1.0); + Rmag.add(3,3,1.0); + getIMUval(); triad(acc_x/accnorm,acc_y/accnorm,acc_z/accnorm, accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,mag_x/magnorm,mag_y/magnorm,mag_z/magnorm, magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm); float sumLPaccnorm = 0; @@ -559,20 +620,6 @@ //姿勢角を更新 getIMUval(); updateBetweenMeasures(); - calcDynAcc(); - th_mg = abs(acos((LPmag_x*LPacc_x+LPmag_y*LPacc_y+LPmag_z*LPacc_z)/LPmagnorm/LPaccnorm)-val_thmg); - dynaccnorm = sqrt(dynacc_x*dynacc_x+dynacc_y*dynacc_y+dynacc_z*dynacc_z); - accnormerr = abs(LPaccnorm-accrefnorm); - //pc.printf("%f %f : %f \r\n",th_mg,th_mgref,abs(th_mg-th_mgref)); - int ang_th = th_mg<0.01; - int dyn_th = dynaccnorm < 0.01; - int norm_th = accnormerr< 0.01; - //pc.printf("%d %d %d %f %f %f\r\n",ang_th,dyn_th,norm_th,abs(th_mg-val_thmg),dynaccnorm ,abs(accnorm-accrefnorm)); - if(dyn_th+ang_th+norm_th>0){ - obs_count += 1; - updateAcrossMeasures(LPmag_x/LPmagnorm,LPmag_y/LPmagnorm,LPmag_z/LPmagnorm,magref[0]/magrefnorm,magref[1]/magrefnorm,magref[2]/magrefnorm,Rmag); - updateAcrossMeasures(LPacc_x/LPaccnorm,LPacc_y/LPaccnorm,LPacc_z/LPaccnorm,accref[0]/accrefnorm,accref[1]/accrefnorm,accref[2]/accrefnorm,Racc); - } computeAngles(); PIDtick.loop();