From 45dca8ae19d434540a2a66baa8d56a51099ef999 Mon Sep 17 00:00:00 2001 From: Kristoffer Skare Date: Tue, 10 Aug 2021 12:42:41 +0200 Subject: [PATCH] Kalman filter modification for constant altitude flight (copied from other repo) --- src/deck/drivers/src/multiranger.c | 24 +++- src/modules/interface/estimator.h | 10 ++ .../interface/kalman_core/kalman_core.h | 9 +- src/modules/interface/kalman_core/mm_flow.h | 3 + src/modules/interface/kalman_core/mm_tof.h | 6 + src/modules/interface/range.h | 9 ++ src/modules/src/estimator_kalman.c | 50 +++++++- src/modules/src/kalman_core/kalman_core.c | 41 ++++++ src/modules/src/kalman_core/mm_flow.c | 68 ++++++++++ src/modules/src/kalman_core/mm_tof.c | 117 ++++++++++++++++++ src/modules/src/range.c | 8 ++ 11 files changed, 341 insertions(+), 4 deletions(-) diff --git a/src/deck/drivers/src/multiranger.c b/src/deck/drivers/src/multiranger.c index cb4ef4fee..ddee160b4 100644 --- a/src/deck/drivers/src/multiranger.c +++ b/src/deck/drivers/src/multiranger.c @@ -43,6 +43,8 @@ #include +#include "cf_math.h" + static bool isInit = false; static bool isTested = false; static bool isPassed = false; @@ -53,12 +55,21 @@ static bool isPassed = false; #define MR_PIN_LEFT PCA95X4_P6 #define MR_PIN_RIGHT PCA95X4_P2 +#define RANGE_UP_OUTLIER_LIMIT 5000 // the measured range is in [mm] + NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devFront; NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devBack; NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devUp; NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devLeft; NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devRight; +// Measurement noise model +static const float expPointA = 2.5f; +static const float expStdA = 0.0025f; // STD at elevation expPointA [m] +static const float expPointB = 4.0f; +static const float expStdB = 0.2f; // STD at elevation expPointB [m] +static float expCoeff; + static bool mrInitSensor(VL53L1_Dev_t *pdev, uint32_t pca95pin, char *name) { bool status; @@ -79,6 +90,10 @@ static bool mrInitSensor(VL53L1_Dev_t *pdev, uint32_t pca95pin, char *name) status = false; } + // pre-compute constant in the measurement noise model for kalman + expCoeff = logf(expStdB / expStdA) / (expPointB - expPointA); + + return status; } @@ -129,12 +144,19 @@ static void mrTask(void *param) while (1) { vTaskDelayUntil(&lastWakeTime, M2T(100)); - + float distanceUp = mrGetMeasurementAndRestart(&devUp)/1000.0f; rangeSet(rangeFront, mrGetMeasurementAndRestart(&devFront)/1000.0f); rangeSet(rangeBack, mrGetMeasurementAndRestart(&devBack)/1000.0f); + rangeSet(rangeUp, distanceUp); rangeSet(rangeUp, mrGetMeasurementAndRestart(&devUp)/1000.0f); rangeSet(rangeLeft, mrGetMeasurementAndRestart(&devLeft)/1000.0f); rangeSet(rangeRight, mrGetMeasurementAndRestart(&devRight)/1000.0f); + + // Add up range to kalman filter measurements + if (distanceUp < RANGE_UP_OUTLIER_LIMIT) { + float stdDev = expStdA * (1.0f + expf( expCoeff * (distanceUp - expPointA))); + rangeEnqueueUpRangeInEstimator(distanceUp, stdDev, xTaskGetTickCount()); + } } } diff --git a/src/modules/interface/estimator.h b/src/modules/interface/estimator.h index a2195d17c..6d22bcfca 100644 --- a/src/modules/interface/estimator.h +++ b/src/modules/interface/estimator.h @@ -43,6 +43,7 @@ typedef enum { MeasurementTypePose, MeasurementTypeDistance, MeasurementTypeTOF, + MeasurementTypeUpTOF, MeasurementTypeAbsoluteHeight, MeasurementTypeFlow, MeasurementTypeYawError, @@ -62,6 +63,7 @@ typedef struct poseMeasurement_t pose; distanceMeasurement_t distance; tofMeasurement_t tof; + tofMeasurement_t uptof; heightMeasurement_t height; flowMeasurement_t flow; yawErrorMeasurement_t yawError; @@ -123,6 +125,14 @@ static inline void estimatorEnqueueTOF(const tofMeasurement_t *tof) estimatorEnqueue(&m); } +static inline void estimatorEnqueueUpTOF(const tofMeasurement_t *tof) +{ + measurement_t m; + m.type = MeasurementTypeUpTOF; + m.data.uptof = *tof; + estimatorEnqueue(&m); +} + static inline void estimatorEnqueueAbsoluteHeight(const heightMeasurement_t *height) { measurement_t m; diff --git a/src/modules/interface/kalman_core/kalman_core.h b/src/modules/interface/kalman_core/kalman_core.h index 3eb84cfc1..4c174650f 100644 --- a/src/modules/interface/kalman_core/kalman_core.h +++ b/src/modules/interface/kalman_core/kalman_core.h @@ -59,7 +59,7 @@ // Indexes to access the quad's state, stored as a column vector typedef enum { - KC_STATE_X, KC_STATE_Y, KC_STATE_Z, KC_STATE_PX, KC_STATE_PY, KC_STATE_PZ, KC_STATE_D0, KC_STATE_D1, KC_STATE_D2, KC_STATE_DIM + KC_STATE_X, KC_STATE_Y, KC_STATE_Z, KC_STATE_PX, KC_STATE_PY, KC_STATE_PZ, KC_STATE_D0, KC_STATE_D1, KC_STATE_D2, KC_STATE_F, KC_STATE_R, KC_STATE_DIM } kalmanCoreStateIdx_t; @@ -74,6 +74,10 @@ typedef struct { * - D0, D1, D2: attitude error * * For more information, refer to the paper + * + * Add two states for better z estimation using range measurements: + * - F: Estimated distance from the starting z position to what is below the CF + * - R: Estimated distance from the starting z position to the current roof above the CF */ float S[KC_STATE_DIM]; @@ -93,6 +97,9 @@ typedef struct { bool resetEstimation; float baroReferenceHeight; + + // Flag to signal if R (roof height estimate) has been intialized + bool stateRInitialized; } kalmanCoreData_t; diff --git a/src/modules/interface/kalman_core/mm_flow.h b/src/modules/interface/kalman_core/mm_flow.h index a4df03aa0..888b541d9 100644 --- a/src/modules/interface/kalman_core/mm_flow.h +++ b/src/modules/interface/kalman_core/mm_flow.h @@ -29,3 +29,6 @@ // Measurements of flow (dnx, dny) void kalmanCoreUpdateWithFlow(kalmanCoreData_t* this, const flowMeasurement_t *flow, const Axis3f *gyro); + +// Kalman update with optical flow measurements using F (floor height estimate) +void kalmanCoreUpdateWithFlowUsingF(kalmanCoreData_t* this, const flowMeasurement_t *flow, const Axis3f *gyro); diff --git a/src/modules/interface/kalman_core/mm_tof.h b/src/modules/interface/kalman_core/mm_tof.h index 46d0bc9a3..e81678f8a 100644 --- a/src/modules/interface/kalman_core/mm_tof.h +++ b/src/modules/interface/kalman_core/mm_tof.h @@ -29,3 +29,9 @@ // Measurements of TOF from laser sensor void kalmanCoreUpdateWithTof(kalmanCoreData_t* this, tofMeasurement_t *tof); + +// Kalman update using the TOF from downward laser sensor and F (floor height estimate) +void kalmanCoreUpdateWithTofUsingF(kalmanCoreData_t* this, tofMeasurement_t *tof); + +// Kalman update using the TOF from upward laser sensor and R (roof height estimate) +void kalmanCoreUpdateWithUpTofUsingR(kalmanCoreData_t* this, tofMeasurement_t *tof); diff --git a/src/modules/interface/range.h b/src/modules/interface/range.h index 3fa8708ef..882078f37 100644 --- a/src/modules/interface/range.h +++ b/src/modules/interface/range.h @@ -61,3 +61,12 @@ float rangeGet(rangeDirection_t direction); * @param timeStamp The time when the range was sampled (in sys ticks) */ void rangeEnqueueDownRangeInEstimator(float distance, float stdDev, uint32_t timeStamp); + +/** + * Enqueue a upward range measurement for distance to the roof in the current estimator. + * + * @param distance Distance to the roof (m) + * @param stdDev The standard deviation of the range sample + * @param timeStamp The time when the range was sampled (in sys ticks) + */ +void rangeEnqueueUpRangeInEstimator(float distance, float stdDev, uint32_t timeStamp); diff --git a/src/modules/src/estimator_kalman.c b/src/modules/src/estimator_kalman.c index c79e33f05..4e7db3d58 100644 --- a/src/modules/src/estimator_kalman.c +++ b/src/modules/src/estimator_kalman.c @@ -120,6 +120,9 @@ static StaticSemaphore_t dataMutexBuffer; static bool robustTwr = false; static bool robustTdoa = false; +// Nonzero to take advantage of the extended state (F and R) to get a better estimate of Z +static bool useFAndR = true; + /** * Quadrocopter State * @@ -129,6 +132,10 @@ static bool robustTdoa = false; * - D0, D1, D2: attitude error * * For more information, refer to the paper + * + * Add two states for better z estimation using range measurements: + * - F: Estimated distance from the starting z position to what is below the CF + * - R: Estimated distance from the starting z position to the current roof above the CF */ NO_DMA_CCM_SAFE_ZERO_INIT static kalmanCoreData_t coreData; @@ -372,15 +379,34 @@ static bool updateQueuedMeasurements(const uint32_t tick) { doneUpdate = true; break; case MeasurementTypeTOF: - kalmanCoreUpdateWithTof(&coreData, &m.data.tof); + if(useFAndR){ + // Tof update using the estimated height of the floor (f) + kalmanCoreUpdateWithTofUsingF(&coreData, &m.data.tof); + }else{ + // Standard Tof update + kalmanCoreUpdateWithTof(&coreData, &m.data.tof); + } doneUpdate = true; break; + case MeasurementTypeUpTOF: + if(useFAndR){ + // Tof update using the upward range measurement and the estimated height of the roof (r) + kalmanCoreUpdateWithUpTofUsingR(&coreData, &m.data.tof); + doneUpdate = true; + } + break; case MeasurementTypeAbsoluteHeight: kalmanCoreUpdateWithAbsoluteHeight(&coreData, &m.data.height); doneUpdate = true; break; case MeasurementTypeFlow: - kalmanCoreUpdateWithFlow(&coreData, &m.data.flow, &gyroLatest); + if(useFAndR){ + // Flow update using the estimated height of the floor + kalmanCoreUpdateWithFlowUsingF(&coreData, &m.data.flow, &gyroLatest); + }else{ + // Standard flow update + kalmanCoreUpdateWithFlow(&coreData, &m.data.flow, &gyroLatest); + } doneUpdate = true; break; case MeasurementTypeYawError: @@ -506,6 +532,14 @@ LOG_GROUP_START(kalman) */ LOG_ADD(LOG_FLOAT, stateD2, &coreData.S[KC_STATE_D2]) /** + * @brief Estimated floor height compared to the reference point where Z = 0 + */ + LOG_ADD(LOG_FLOAT, stateF, &coreData.S[KC_STATE_F]) + /** + * @brief Estimated roof height compared to the reference point where Z = 0 + */ + LOG_ADD(LOG_FLOAT, stateR, &coreData.S[KC_STATE_R]) + /** * @brief Covariance matrix position x */ LOG_ADD(LOG_FLOAT, varX, &coreData.P[KC_STATE_X][KC_STATE_X]) @@ -542,6 +576,14 @@ LOG_GROUP_START(kalman) */ LOG_ADD(LOG_FLOAT, varD2, &coreData.P[KC_STATE_D2][KC_STATE_D2]) /** + * @brief Covariance matrix floor height estimate + */ + LOG_ADD(LOG_FLOAT, varF, &coreData.P[KC_STATE_F][KC_STATE_F]) + /** + * @brief Covariance matrix roof height estimate + */ + LOG_ADD(LOG_FLOAT, varR, &coreData.P[KC_STATE_R][KC_STATE_R]) + /** * @brief Estimated Attitude quarternion w */ LOG_ADD(LOG_FLOAT, q0, &coreData.q[0]) @@ -593,4 +635,8 @@ PARAM_GROUP_START(kalman) * @brief Nonzero to use robust TWR method (default: 0) */ PARAM_ADD_CORE(PARAM_UINT8, robustTwr, &robustTwr) +/** + * @brief Nonzero to use F (floor height) and R (roof height) in the estimation for a better Z estimate + */ + PARAM_ADD_CORE(PARAM_UINT8, useFAndR, &useFAndR) PARAM_GROUP_STOP(kalman) diff --git a/src/modules/src/kalman_core/kalman_core.c b/src/modules/src/kalman_core/kalman_core.c index b89c43287..cbf5af3eb 100644 --- a/src/modules/src/kalman_core/kalman_core.c +++ b/src/modules/src/kalman_core/kalman_core.c @@ -131,6 +131,11 @@ static const float stdDevInitialVelocity = 0.01; static const float stdDevInitialAttitude_rollpitch = 0.01; static const float stdDevInitialAttitude_yaw = 0.01; +static const float stdDevInitialF = 0; // initialF is almost true by definition +static const float stdDevInitialR = 0; +static float procNoiseF = 0; // This could be tuned to some other value, but it seems natural for it to be 0 +static float procNoiseR = 0; // This could be tuned to some other value, but it seems natural for it to be 0 + static float procNoiseAcc_xy = 0.5f; static float procNoiseAcc_z = 1.0f; static float procNoiseVel = 0; @@ -144,6 +149,9 @@ static float initialX = 0.0; static float initialY = 0.0; static float initialZ = 0.0; +static float initialF = 0.0; +static float initialR = -1.0; // It does not matter since it is overwritten on the first range up measurement (as long as stateRInitialized is set to false). + // Initial yaw of the Crazyflie in radians. // 0 --- facing positive X // PI / 2 --- facing positive Y @@ -168,6 +176,11 @@ void kalmanCoreInit(kalmanCoreData_t* this) { // this->S[KC_STATE_D0] = 0; // this->S[KC_STATE_D1] = 0; // this->S[KC_STATE_D2] = 0; + + this->S[KC_STATE_F] = initialF; + this->S[KC_STATE_R] = initialR; + // This is set to false so that the first range up measurement will become the initial value + this->stateRInitialized = false; // reset the attitude quaternion initialQuaternion[0] = arm_cos_f32(initialYaw / 2); @@ -200,6 +213,9 @@ void kalmanCoreInit(kalmanCoreData_t* this) { this->P[KC_STATE_D1][KC_STATE_D1] = powf(stdDevInitialAttitude_rollpitch, 2); this->P[KC_STATE_D2][KC_STATE_D2] = powf(stdDevInitialAttitude_yaw, 2); + this->P[KC_STATE_F][KC_STATE_F] = powf(stdDevInitialF, 2); + this->P[KC_STATE_R][KC_STATE_R] = powf(stdDevInitialR, 2); + this->Pm.numRows = KC_STATE_DIM; this->Pm.numCols = KC_STATE_DIM; this->Pm.pData = (float*)this->P; @@ -380,6 +396,9 @@ void kalmanCorePredict(kalmanCoreData_t* this, Axis3f *acc, Axis3f *gyro, float A[KC_STATE_D1][KC_STATE_D1] = 1; A[KC_STATE_D2][KC_STATE_D2] = 1; + A[KC_STATE_F][KC_STATE_F] = 1; + A[KC_STATE_R][KC_STATE_R] = 1; + // position from body-frame velocity A[KC_STATE_X][KC_STATE_PX] = this->R[0][0]*dt; A[KC_STATE_Y][KC_STATE_PX] = this->R[1][0]*dt; @@ -581,6 +600,9 @@ void kalmanCoreAddProcessNoise(kalmanCoreData_t* this, float dt) this->P[KC_STATE_D0][KC_STATE_D0] += powf(measNoiseGyro_rollpitch * dt + procNoiseAtt, 2); this->P[KC_STATE_D1][KC_STATE_D1] += powf(measNoiseGyro_rollpitch * dt + procNoiseAtt, 2); this->P[KC_STATE_D2][KC_STATE_D2] += powf(measNoiseGyro_yaw * dt + procNoiseAtt, 2); + + this->P[KC_STATE_F][KC_STATE_F] += powf(procNoiseF, 2); + this->P[KC_STATE_R][KC_STATE_R] += powf(procNoiseR, 2); } for (int i=0; istdDevY); } +void kalmanCoreUpdateWithFlowUsingF(kalmanCoreData_t* this, const flowMeasurement_t *flow, const Axis3f *gyro) +{ + // Inclusion of flow measurements in the EKF done by two scalar updates + + // ~~~ Camera constants ~~~ + // The angle of aperture is guessed from the raw data register and thankfully look to be symmetric + float Npix = 30.0; // [pixels] (same in x and y) + //float thetapix = DEG_TO_RAD * 4.0f; // [rad] (same in x and y) + float thetapix = DEG_TO_RAD * 4.2f; + //~~~ Body rates ~~~ + // TODO check if this is feasible or if some filtering has to be done + float omegax_b = gyro->x * DEG_TO_RAD; + float omegay_b = gyro->y * DEG_TO_RAD; + + // ~~~ Moves the body velocity into the global coordinate system ~~~ + // [bar{x},bar{y},bar{z}]_G = R*[bar{x},bar{y},bar{z}]_B + // + // \dot{x}_G = (R^T*[dot{x}_B,dot{y}_B,dot{z}_B])\dot \hat{x}_G + // \dot{x}_G = (R^T*[dot{x}_B,dot{y}_B,dot{z}_B])\dot \hat{x}_G + // + // where \hat{} denotes a basis vector, \dot{} denotes a derivative and + // _G and _B refer to the global/body coordinate systems. + + // Modification 1 + //dx_g = R[0][0] * S[KC_STATE_PX] + R[0][1] * S[KC_STATE_PY] + R[0][2] * S[KC_STATE_PZ]; + //dy_g = R[1][0] * S[KC_STATE_PX] + R[1][1] * S[KC_STATE_PY] + R[1][2] * S[KC_STATE_PZ]; + + + float dx_g = this->S[KC_STATE_PX]; + float dy_g = this->S[KC_STATE_PY]; + float z_g = (this->S[KC_STATE_Z] - this->S[KC_STATE_F]); + // Saturate elevation in prediction and correction to avoid singularities + if ( z_g < 0.1f ) { + z_g = 0.1; + } + + // ~~~ X velocity prediction and update ~~~ + // predicts the number of accumulated pixels in the x-direction + float omegaFactor = 1.25f; + float hx[KC_STATE_DIM] = {0}; + arm_matrix_instance_f32 Hx = {1, KC_STATE_DIM, hx}; + predictedNX = (flow->dt * Npix / thetapix ) * ((dx_g * this->R[2][2] / z_g) - omegaFactor * omegay_b); + measuredNX = flow->dpixelx; + + // derive measurement equation with respect to dx (not z and f since it caused some bad behaviour) + + hx[KC_STATE_PX] = (Npix * flow->dt / thetapix) * (this->R[2][2] / z_g); + //hx[KC_STATE_Z] = (Npix * flow->dt / thetapix) * ((this->R[2][2] * dx_g) / (-z_g * z_g)); + //hx[KC_STATE_F] = - hx[KC_STATE_Z]; + + //First update + kalmanCoreScalarUpdate(this, &Hx, measuredNX-predictedNX, flow->stdDevX); + + // ~~~ Y velocity prediction and update ~~~ + float hy[KC_STATE_DIM] = {0}; + arm_matrix_instance_f32 Hy = {1, KC_STATE_DIM, hy}; + predictedNY = (flow->dt * Npix / thetapix ) * ((dy_g * this->R[2][2] / z_g) + omegaFactor * omegax_b); + measuredNY = flow->dpixely; + + // derive measurement equation with respect to dy (not z and f since it caused some bad behaviour) + hy[KC_STATE_PY] = (Npix * flow->dt / thetapix) * (this->R[2][2] / z_g); + //hy[KC_STATE_Z] = (Npix * flow->dt / thetapix) * ((this->R[2][2] * dy_g) / (-z_g * z_g)); + //hy[KC_STATE_F] = - hy[KC_STATE_Z]; + + // Second update + kalmanCoreScalarUpdate(this, &Hy, measuredNY-predictedNY, flow->stdDevY); +} + /** * Predicted and measured values of the X and Y direction of the flowdeck */ diff --git a/src/modules/src/kalman_core/mm_tof.c b/src/modules/src/kalman_core/mm_tof.c index 9d4b49280..07249f83e 100644 --- a/src/modules/src/kalman_core/mm_tof.c +++ b/src/modules/src/kalman_core/mm_tof.c @@ -24,6 +24,15 @@ */ #include "mm_tof.h" +#include "param.h" + +// Parameters for tuning the detection for f and r estimation in the Tof update. +// Factor multipled with the standard deviation of the measurement and compared to the prediction error (This is an int because of problems with param, if it is solved then it should probably be canged to a float) +static int detection_factor = 10; +// The value the variance of f or r is set to when a detection happenes. It can probably be tuned to be smaller, but there it does not really seem to matter as long as it is "large enough" +static float variance_after_detection = 50; +// Flag for turning the detection on and off. Without detection f and r tend to not change, causing the same problems as when not using them at all. There could be some way to get it to work without detection, but it is not implemented. +static bool use_detection = true; void kalmanCoreUpdateWithTof(kalmanCoreData_t* this, tofMeasurement_t *tof) { @@ -51,3 +60,111 @@ void kalmanCoreUpdateWithTof(kalmanCoreData_t* this, tofMeasurement_t *tof) kalmanCoreScalarUpdate(this, &H, measuredDistance-predictedDistance, tof->stdDev); } } + +void kalmanCoreUpdateWithTofUsingF(kalmanCoreData_t* this, tofMeasurement_t *tof) +{ + // Updates the filter with a measured distance in the zb direction using the + float h[KC_STATE_DIM] = {0}; + arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h}; + + // Only update the filter if the measurement is reliable (\hat{h} -> infty when R[2][2] -> 0) + if (fabs(this->R[2][2]) > 0.1 && this->R[2][2] > 0){ + float angle = fabsf(acosf(this->R[2][2])) - DEG_TO_RAD * (15.0f / 2.0f); + if (angle < 0.0f) { + angle = 0.0f; + } + //float predictedDistance = S[KC_STATE_Z] / cosf(angle); + float predictedDistance = (this->S[KC_STATE_Z]-this->S[KC_STATE_F]) / this->R[2][2]; + float measuredDistance = tof->distance; // [m] + + float error = measuredDistance-predictedDistance; + + + if (use_detection){ + float threshold = detection_factor*(tof->stdDev); + // If the error is very large it probably means that S[KC_STATE_F] needs to change + if(error*error > threshold*threshold){ + // Give a best first guess of the new floor height and set the variance high + this->P[KC_STATE_F][KC_STATE_F] = variance_after_detection; + this->S[KC_STATE_F] = this->S[KC_STATE_Z] - measuredDistance*this->R[2][2]; + error = 0; + } + } + + //Measurement equation + // + // h = (z - f)/((R*z_b)\dot z_b) = z/cos(alpha) + h[KC_STATE_Z] = 1 / this->R[2][2]; + //h[KC_STATE_Z] = 1 / cosf(angle); + + h[KC_STATE_F] = -1 / this->R[2][2]; + + // Scalar update + kalmanCoreScalarUpdate(this, &H, error, tof->stdDev); + } +} + +void kalmanCoreUpdateWithUpTofUsingR(kalmanCoreData_t* this, tofMeasurement_t *tof) +{ + // Updates the filter with a measured distance in the zb direction using the + float h[KC_STATE_DIM] = {0}; + arm_matrix_instance_f32 H = {1, KC_STATE_DIM, h}; + + // + if(!this->stateRInitialized){ + this->S[KC_STATE_R] = tof->distance - this->S[KC_STATE_Z]; // Just set it equal to the measurement + this->stateRInitialized = true; + } + + + // Only update the filter if the measurement is reliable (\hat{h} -> infty when R[2][2] -> 0) + if (fabs(this->R[2][2]) > 0.1 && this->R[2][2] > 0){ + float angle = fabsf(acosf(this->R[2][2])) - DEG_TO_RAD * (15.0f / 2.0f); + if (angle < 0.0f) { + angle = 0.0f; + } + //float predictedDistance = S[KC_STATE_Z] / cosf(angle); + float predictedDistance = (this->S[KC_STATE_R] - this->S[KC_STATE_Z]) / this->R[2][2]; + float measuredDistance = tof->distance; // [m] + + float error = measuredDistance-predictedDistance; + + if (use_detection){ + float threshold = detection_factor*(tof->stdDev); + // If the error is very large it probably means that S[KC_STATE_R] needs to change + if(error*error > threshold*threshold){ + // Give a best first guess of the new roof height and set the variance high + this->P[KC_STATE_R][KC_STATE_R] = variance_after_detection; + this->S[KC_STATE_R] = measuredDistance*this->R[2][2] + this->S[KC_STATE_Z]; + error = 0; + } + } + + //Measurement equation + // + // h = (r - z)/((R*z_b)\dot z_b) = z/cos(alpha) + h[KC_STATE_Z] = -1 / this->R[2][2]; + //h[KC_STATE_Z] = -1 / cosf(angle); + + h[KC_STATE_R] = 1 / this->R[2][2]; + + // Scalar update + kalmanCoreScalarUpdate(this, &H, error, tof->stdDev); + } +} + +PARAM_GROUP_START(kalman) +/** + * @brief The error threshold in Tof measurements that cause a detection of step in F or R + */ + PARAM_ADD_CORE(PARAM_UINT8, tofDetectionFactor, &detection_factor) + /** + * @brief The variance used in the covariance matrix when a detection happenes + */ + //PARAM_ADD_CORE(PARAM_FLOAT, varianceAfterDetection, &variance_after_detection) + /** + * @brief Non zero to use detection in Tof measurements for changes in F or R + */ + //PARAM_ADD_CORE(PARAM_UINT8, useDetection, &use_detection) + +PARAM_GROUP_STOP(kalman) diff --git a/src/modules/src/range.c b/src/modules/src/range.c index a70138b75..632036fbe 100644 --- a/src/modules/src/range.c +++ b/src/modules/src/range.c @@ -56,6 +56,14 @@ void rangeEnqueueDownRangeInEstimator(float distance, float stdDev, uint32_t tim estimatorEnqueueTOF(&tofData); } +void rangeEnqueueUpRangeInEstimator(float distance, float stdDev, uint32_t timeStamp) { + tofMeasurement_t tofData; + tofData.timestamp = timeStamp; + tofData.distance = distance; + tofData.stdDev = stdDev; + estimatorEnqueueUpTOF(&tofData); +} + /** * Log group for the multi ranger and Z-ranger decks */