Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: main.cpp
- Revision:
- 37:dad55cf05e3d
- Parent:
- 36:e68ce293306e
- Child:
- 38:acc7cdcf56dd
diff -r e68ce293306e -r dad55cf05e3d main.cpp --- a/main.cpp Wed Mar 03 03:34:11 2021 +0000 +++ b/main.cpp Wed Mar 03 09:32:10 2021 +0000 @@ -39,6 +39,7 @@ Matrix qhat(4,1); Matrix Phat(4,4); +Matrix Qgyro(3,3); Matrix Racc(3,3); @@ -150,7 +151,7 @@ //MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z); } -void updateBetweenMeasures(){ +void getIMUval(){ // gx gy gz ax ay az accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); ax = ax - offsetVal[0]; @@ -160,14 +161,18 @@ gy = gy - offsetVal[4]; gz = gz - offsetVal[5]; // 加速度値を分解能で割って加速度(G)に変換する - acc_x = ax / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g - acc_y = ay / ACCEL_SSF; - acc_z = az / ACCEL_SSF; + acc_x = float(ax) / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g + acc_y = float(ay) / ACCEL_SSF; + acc_z = float(az) / ACCEL_SSF; // 角速度値を分解能で割って角速度(rad per sec)に変換する gyro_x = float(gx) / GYRO_SSF * 0.0174533f; // (rad/s) gyro_y = float(gy) / GYRO_SSF * 0.0174533f; gyro_z = float(gz) / GYRO_SSF * 0.0174533f; +} + +void updateBetweenMeasures(){ + getIMUval(); Matrix phi(4,4); phi << 1.0 << -gyro_x*0.5*att_dt <<-gyro_y*0.5*att_dt <<-gyro_z*0.5*att_dt @@ -179,6 +184,95 @@ float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); qhat *= (1.0f/ qnorm); + float q0 = qhat.getNumber( 1, 1 ); + float q1 = qhat.getNumber( 2, 1 ); + float q2 = qhat.getNumber( 3, 1 ); + float q3 = qhat.getNumber( 4, 1 ); + + Matrix B(4,3); + B << q1 << q2 << q3 + <<-q0 << q3 <<-q2 + <<-q3 <<-q0 << q1 + << q2 <<-q1 <<-q0; + B *= 0.5f; + Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B); + + +} + +void updateAcrossMeasures(){ + getIMUval(); + + Matrix uacc(3,1); + Matrix vacc(3,1); + + uacc << 0 << 0 << -1; + float accnorm = sqrt(acc_x*acc_x+acc_y*acc_y+acc_z*acc_z); + vacc << acc_x/accnorm << acc_y/accnorm << acc_z/accnorm; + + float q0 = qhat.getNumber( 1, 1 ); + float q1 = qhat.getNumber( 2, 1 ); + float q2 = qhat.getNumber( 3, 1 ); + float q3 = qhat.getNumber( 4, 1 ); + + Matrix A1(3,3); + A1 << q0 << q3 << -q2 + <<-q3 << q0 << q1 + <<q2 <<-q1 <<q0; + A1 *= 2.0f; + + Matrix A2(3,3); + A2 << q1 << q2 << q3 + << q2 <<-q1 << q0 + << q3 <<-q0 <<-q1; + A2 *= 2.0f; + + Matrix A3(3,3); + A3 <<-q2 << q1 <<-q0 + << q1 << q2 << q3 + << q0 << q3 <<-q2; + A3 *= 2.0f; + + Matrix A4(3,3); + A4 <<-q3 << q0 << q1 + <<-q0 <<-q3 << q2 + << q1 << q2 << q3; + A4 *= 2.0f; + + Matrix H(3,4); + + Matrix ab1(A1*uacc); + Matrix ab2(A2*uacc); + Matrix ab3(A3*uacc); + Matrix ab4(A4*uacc); + + H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 ) + << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 ) + << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 ); + + Matrix D(3,3); + D << q0*q0 + q1*q1 - q2*q2 - q3*q3 + << 2*(q1*q2 + q0*q3) + << 2*(q1*q3 - q0*q2) + << 2*(q1*q2 - q0*q3) + << q0*q0 - q1*q1 + q2*q2 - q3*q3 + << 2*(q2*q3 + q0*q1) + << 2*(q1*q3 + q0*q2) + << 2*(q2*q3 - q0*q1) + << q0*q0 - q1*q1 - q2*q2 + q3*q3; + + Matrix err(3,1); + err = vacc-D*uacc; + + Matrix K(4,3); + K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Racc); + + Matrix dq(4,1); + dq = K*err; + qhat = qhat-dq; + + float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); + qhat *= (1.0f/ qnorm); } void computeAngles() @@ -224,7 +318,11 @@ << 0 << 0.0001 <<0 <<0 << 0 << 0 <<0.0001 <<0 << 0 << 0 << 0 << 0.0001; - + + Qgyro << 0.0001 << 0 <<0 + << 0 << 0.0001 <<0 + << 0 << 0 <<0.0001; + Racc << 0.0001 << 0 <<0 << 0 << 0.0001 <<0 << 0 << 0 <<0.0001; @@ -232,7 +330,7 @@ LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt); - //pc.baud(115200); + pc.baud(115200); //sd.baud(115200); accelgyro.initialize(); //加速度計のフルスケールレンジを設定 @@ -249,6 +347,7 @@ //姿勢角を更新 //updateAttitude(); updateBetweenMeasures(); + updateAcrossMeasures(); computeAngles(); PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する