diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h index 374cd99654..69c26e9d12 100644 --- a/src/modules/interface/stabilizer_types.h +++ b/src/modules/interface/stabilizer_types.h @@ -247,14 +247,13 @@ typedef struct { /** Sweep angle measurement */ typedef struct { uint32_t timestamp; - vec3d* baseStationPos; - mat3d* baseStationRot; // Base station rotation matrix - mat3d* baseStationRotInv; // Inverted base station rotation matrix - float angleX; - float angleY; - float stdDevX; - float stdDevY; - vec3d* sensorPos; + vec3d* sensorPos; // Sensor position in the CF reference frame + vec3d* rotorPos; // Pos of rotor origin in global reference frame + mat3d* rotorRot; // Rotor rotation matrix + mat3d* rotorRotInv; // Inverted rotor rotation matrix + float tan_t; // t is the tilt angle of the light plane on the rotor + float measuredSweepAngle; + float stdDev; } sweepAngleMeasurement_t; // Frequencies to bo used with the RATE_DO_EXECUTE_HZ macro. Do NOT use an arbitrary number. diff --git a/src/modules/src/kalman_core.c b/src/modules/src/kalman_core.c index 9af3b9bfd8..83b7f42fec 100644 --- a/src/modules/src/kalman_core.c +++ b/src/modules/src/kalman_core.c @@ -571,82 +571,67 @@ void kalmanCoreUpdateWithYawError(kalmanCoreData_t *this, yawErrorMeasurement_t scalarUpdate(this, &H, this->S[KC_STATE_D2] - error->yawError, error->stdDev); } -static void scalarUpdateForSweep(kalmanCoreData_t *this, float measuredSweepAngle, float dp, float dx, kalmanCoreStateIdx_t state_p, float stdDev, arm_matrix_instance_f32* R, float distanceToBs, const uint32_t tick) { - if(dx != 0) { - float predictedSweepAngle = atan2(dp, dx); +void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *angles, const uint32_t tick) { + // Rotate the sensor position from CF reference frame to global reference frame, + // using the CF roatation matrix + vec3d s; + arm_matrix_instance_f32 Rcf_ = {3, 3, (float32_t *)this->R}; + arm_matrix_instance_f32 scf_ = {3, 1, *angles->sensorPos}; + arm_matrix_instance_f32 s_ = {3, 1, s}; + mat_mult(&Rcf_, &scf_, &s_); + + // Get the current state values of the position of the crazyflie (global reference frame) and add the relative sensor pos + vec3d pcf = {this->S[KC_STATE_X] + s[0], this->S[KC_STATE_Y] + s[1], this->S[KC_STATE_Z] + s[2]}; + + // Calculate the difference between the rotor and the sensor on the CF (global reference frame) + const vec3d* pr = angles->rotorPos; + vec3d stmp = {pcf[0] - (*pr)[0], pcf[1] - (*pr)[1], pcf[2] - (*pr)[2]}; + arm_matrix_instance_f32 stmp_ = {3, 1, stmp}; + + // Rotate the difference in position to the rotor reference frame, + // using the rotor inverse rotation matrix + vec3d sr; + arm_matrix_instance_f32 Rr_inv_ = {3, 3, (float32_t *)(*angles->rotorRotInv)}; + arm_matrix_instance_f32 sr_ = {3, 1, sr}; + mat_mult(&Rr_inv_, &stmp_, &sr_); + + // The following computations are in the rotor refernece frame + const float x = sr[0]; + const float y = sr[1]; + const float z = sr[2]; + const float tan_t = angles->tan_t; + + const float r2 = x * x + y * y; + const float r = arm_sqrt(r2); + const float z_tan_t = z * tan_t; + + const float predictedSweepAngle = atan2(y, x) + asin(z_tan_t / r); + const float measuredSweepAngle = angles->measuredSweepAngle; + const float error = measuredSweepAngle - predictedSweepAngle; + + if (outlierFilterValidateLighthouseSweep(&sweepOutlierFilterState, r, error, tick)) { + // Calculate H vector (in the rotor reference frame) + const float q = tan_t / arm_sqrt(r2 - z_tan_t * z_tan_t); + vec3d gr = {(-y - x * z * q) / r2, (x - y * z * q) / r2 , q}; + + // gr is in the rotor reference frame, rotate back to the global + // reference frame using the rotor rotation matrix + vec3d g; + arm_matrix_instance_f32 gr_ = {3, 1, gr}; + arm_matrix_instance_f32 Rr_ = {3, 3, (float32_t *)(*angles->rotorRot)}; + arm_matrix_instance_f32 g_ = {3, 1, g}; + mat_mult(&Rr_, &gr_, &g_); - float angleError = measuredSweepAngle - predictedSweepAngle; - if (outlierFilterValidateLighthouseSweep(&sweepOutlierFilterState, distanceToBs, angleError, tick)) { - float n = (dx * dx + dp * dp); - - float h[KC_STATE_DIM] = {0}; - arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h}; - - // Rotate back to global coordinate system - vec3d h_b = {0, 0, 0}; - arm_matrix_instance_f32 H_B = {3, 1, h_b}; - h_b[KC_STATE_X] = -dp / n; - h_b[state_p] = dx / n; - - vec3d h_g = {0, 0, 0}; - arm_matrix_instance_f32 H_G = {3, 1, h_g}; - - mat_mult(R, &H_B, &H_G); - - h[KC_STATE_X] = h_g[0]; - h[KC_STATE_Y] = h_g[1]; - h[KC_STATE_Z] = h_g[2]; + float h[KC_STATE_DIM] = {0}; + h[KC_STATE_X] = g[0]; + h[KC_STATE_Y] = g[1]; + h[KC_STATE_Z] = g[2]; - scalarUpdate(this, &H, angleError, stdDev); - } + arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h}; + scalarUpdate(this, &H, error, angles->stdDev); } } -void kalmanCoreUpdateWithSweepAngles(kalmanCoreData_t *this, sweepAngleMeasurement_t *angles, const uint32_t tick) -{ - // Get rotation matrix and invert it (to get the global to local rotation matrix) - arm_matrix_instance_f32 basestation_rotation_matrix = {3, 3, (float32_t *)(*angles->baseStationRot)}; - arm_matrix_instance_f32 basestation_rotation_matrix_inv = {3, 3, (float32_t *)(*angles->baseStationRotInv)}; - - // Rotate the sensor position using the CF roatation matrix, to rotate it to global coordinates - arm_matrix_instance_f32 CF_ROT_MATRIX = {3, 3, (float32_t *)this->R}; - arm_matrix_instance_f32 SENSOR_RELATVIVE_POS = {3, 1, *angles->sensorPos}; - vec3d sensor_relative_pos_glob = {0}; - arm_matrix_instance_f32 SENSOR_RELATVIVE_POS_GLOB = {3, 1, sensor_relative_pos_glob}; - mat_mult(&CF_ROT_MATRIX, &SENSOR_RELATVIVE_POS, &SENSOR_RELATVIVE_POS_GLOB); - - // Get the current state values of the position of the crazyflie and add the relative sensor pos - float pos_x = this->S[KC_STATE_X] + sensor_relative_pos_glob[0]; - float pos_y = this->S[KC_STATE_Y] + sensor_relative_pos_glob[1]; - float pos_z = this->S[KC_STATE_Z] + sensor_relative_pos_glob[2]; - - // Calculate the difference between the base stations and the sensor on the CF. - const float* baseStationPos = *angles->baseStationPos; - float dx = pos_x - baseStationPos[0]; - float dy = pos_y - baseStationPos[1]; - float dz = pos_z - baseStationPos[2]; - - // Rotate the difference in position to be relative to the basestation - vec3d position_diff = {dx, dy, dz}; - vec3d position_diff_rotated = {0, 0, 0}; - arm_matrix_instance_f32 vec_pos_diff = {3, 1, position_diff}; - arm_matrix_instance_f32 vec_pos_diff_rot = {3, 1, position_diff_rotated}; - mat_mult(&basestation_rotation_matrix_inv, &vec_pos_diff, &vec_pos_diff_rot); - - float dx_rot = position_diff_rotated[0]; - float dy_rot = position_diff_rotated[1]; - float dz_rot = position_diff_rotated[2]; - - // Retrieve the measured sweepangles - float measuredSweepAngleHorizontal = angles->angleX; - float measuredSweepAngleVertical = angles->angleY; - - float distanceToBs = arm_sqrt(dx * dx + dy * dy + dz * dz); - - scalarUpdateForSweep(this, measuredSweepAngleHorizontal, dy_rot, dx_rot, KC_STATE_Y, angles->stdDevX, &basestation_rotation_matrix, distanceToBs, tick); - scalarUpdateForSweep(this, measuredSweepAngleVertical, dz_rot, dx_rot, KC_STATE_Z, angles->stdDevY, &basestation_rotation_matrix, distanceToBs, tick); -} - void kalmanCorePredict(kalmanCoreData_t* this, float cmdThrust, Axis3f *acc, Axis3f *gyro, float dt, bool quadIsFlying) { /* Here we discretize (euler forward) and linearise the quadrocopter dynamics in order diff --git a/src/modules/src/lighthouse/lighthouse_position_est.c b/src/modules/src/lighthouse/lighthouse_position_est.c index 2ca3e16a4f..3ea8de58f6 100644 --- a/src/modules/src/lighthouse/lighthouse_position_est.c +++ b/src/modules/src/lighthouse/lighthouse_position_est.c @@ -49,26 +49,39 @@ static STATS_CNT_RATE_DEFINE(estBs1Rate, HALF_SECOND); static statsCntRateLogger_t* bsEstRates[PULSE_PROCESSOR_N_BASE_STATIONS] = {&estBs0Rate, &estBs1Rate}; baseStationEulerAngles_t lighthouseBaseStationAngles[PULSE_PROCESSOR_N_BASE_STATIONS]; + +// Pre calculated data used in state updates static mat3d baseStationInvertedRotationMatrixes[PULSE_PROCESSOR_N_BASE_STATIONS]; +static mat3d lh1Rotor2RotationMatrixes[PULSE_PROCESSOR_N_BASE_STATIONS]; +static mat3d lh1Rotor2InvertedRotationMatrixes[PULSE_PROCESSOR_N_BASE_STATIONS]; -static void invertRotationMatrix(mat3d rot, mat3d inverted); +static void preProcessGeometryData(mat3d bsRot, mat3d bsRotInverted, mat3d lh1Rotor2Rot, mat3d lh1Rotor2RotInverted); void lightHousePositionGeometryDataUpdated() { for (int i = 0; i < PULSE_PROCESSOR_N_BASE_STATIONS; i++) { lighthouseGeometryCalculateAnglesFromRotationMatrix(&lighthouseBaseStationsGeometry[i], &lighthouseBaseStationAngles[i]); - invertRotationMatrix(lighthouseBaseStationsGeometry[i].mat, baseStationInvertedRotationMatrixes[i]); + preProcessGeometryData(lighthouseBaseStationsGeometry[i].mat, baseStationInvertedRotationMatrixes[i], lh1Rotor2RotationMatrixes[i], lh1Rotor2InvertedRotationMatrixes[i]); } } - -static void invertRotationMatrix(mat3d rot, mat3d inverted) { - // arm_mat_inverse_f32() alters the original matrix in the process, must make a copy to work from - float bs_r_tmp[3][3]; - memcpy(bs_r_tmp, (float32_t *)rot, sizeof(bs_r_tmp)); - arm_matrix_instance_f32 basestation_rotation_matrix_tmp = {3, 3, (float32_t *)bs_r_tmp}; - - arm_matrix_instance_f32 basestation_rotation_matrix_inv = {3, 3, (float32_t *)inverted}; - arm_mat_inverse_f32(&basestation_rotation_matrix_tmp, &basestation_rotation_matrix_inv); +static void preProcessGeometryData(mat3d bsRot, mat3d bsRotInverted, mat3d lh1Rotor2Rot, mat3d lh1Rotor2RotInverted) { + // For a rotation matrix inverse and transpose is equal. Use transpose instead + arm_matrix_instance_f32 bsRot_ = {3, 3, (float32_t *)bsRot}; + arm_matrix_instance_f32 bsRotInverted_ = {3, 3, (float32_t *)bsRotInverted}; + arm_mat_trans_f32(&bsRot_, &bsRotInverted_); + + // In a LH1 system, the axis of rotation of the second rotor is perpendicular to the first rotor + mat3d secondRotorInvertedR = { + {1, 0, 0}, + {0, 0, -1}, + {0, 1, 0} + }; + arm_matrix_instance_f32 secondRotorInvertedR_ = {3, 3, (float32_t *)secondRotorInvertedR}; + arm_matrix_instance_f32 lh1Rotor2Rot_ = {3, 3, (float32_t *)lh1Rotor2Rot}; + arm_mat_mult_f32(&bsRot_, &secondRotorInvertedR_, &lh1Rotor2Rot_); + + arm_matrix_instance_f32 lh1Rotor2RotInverted_ = {3, 3, (float32_t *)lh1Rotor2RotInverted}; + arm_mat_trans_f32(&lh1Rotor2Rot_, &lh1Rotor2RotInverted_); } @@ -128,21 +141,27 @@ static void estimatePositionSweeps(pulseProcessorResult_t* angles, int baseStati for (size_t sensor = 0; sensor < PULSE_PROCESSOR_N_SENSORS; sensor++) { pulseProcessorBaseStationMeasuremnt_t* bsMeasurement = &angles->sensorMeasurements[sensor].baseStatonMeasurements[baseStation]; if (bsMeasurement->validCount == PULSE_PROCESSOR_N_SWEEPS) { - sweepAngleMeasurement_t sweepAngles; - sweepAngles.angleX = bsMeasurement->correctedAngles[0]; - sweepAngles.angleY = bsMeasurement->correctedAngles[1]; - - if (sweepAngles.angleX != 0 && sweepAngles.angleY != 0) { - sweepAngles.stdDevX = sweepStd; - sweepAngles.stdDevY = sweepStd; - - sweepAngles.sensorPos = &sensorDeckPositions[sensor]; + sweepAngleMeasurement_t sweepInfo; + sweepInfo.sensorPos = &sensorDeckPositions[sensor]; + sweepInfo.stdDev = sweepStd; + sweepInfo.rotorPos = &lighthouseBaseStationsGeometry[baseStation].origin; + sweepInfo.tan_t = 0; + + sweepInfo.measuredSweepAngle = bsMeasurement->correctedAngles[0]; + if (sweepInfo.measuredSweepAngle != 0) { + sweepInfo.rotorRot = &lighthouseBaseStationsGeometry[baseStation].mat; + sweepInfo.rotorRotInv = &baseStationInvertedRotationMatrixes[baseStation]; + + estimatorEnqueueSweepAngles(&sweepInfo); + STATS_CNT_RATE_EVENT(bsEstRates[baseStation]); + } - sweepAngles.baseStationPos = &lighthouseBaseStationsGeometry[baseStation].origin; - sweepAngles.baseStationRot = &lighthouseBaseStationsGeometry[baseStation].mat; - sweepAngles.baseStationRotInv = &baseStationInvertedRotationMatrixes[baseStation]; + sweepInfo.measuredSweepAngle = bsMeasurement->correctedAngles[1]; + if (sweepInfo.measuredSweepAngle != 0) { + sweepInfo.rotorRot = &lh1Rotor2RotationMatrixes[baseStation]; + sweepInfo.rotorRotInv = &lh1Rotor2InvertedRotationMatrixes[baseStation]; - estimatorEnqueueSweepAngles(&sweepAngles); + estimatorEnqueueSweepAngles(&sweepInfo); STATS_CNT_RATE_EVENT(bsEstRates[baseStation]); } }