Skip to content

Commit

Permalink
#570 Reworked measurement model for sweeps in kalman filter
Browse files Browse the repository at this point in the history 
Now supports arbitrary axis of rotation for the rotor and tilt angle for the light plane
  • Loading branch information
@krichardsson
krichardsson committed May 20, 2020
1 parent ce7b901 commit 8787986
Show file tree
Hide file tree
Showing 3 changed files with 106 additions and 103 deletions.
15 changes: 7 additions & 8 deletions src/modules/interface/stabilizer_types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
127 changes: 56 additions & 71 deletions src/modules/src/kalman_core.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
67 changes: 43 additions & 24 deletions src/modules/src/lighthouse/lighthouse_position_est.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
}


Expand Down Expand Up @@ -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]);
}
}
Expand Down

0 comments on commit 8787986

Please sign in to comment.