Non dynamic biquad/refactored sharpness (#10)

* new sharpness new sharpness nondynamic biquad * update the sharpness code make it compile again. * Update version.h * version as imuf_250 Co-authored-by: nerdCopter <[email protected]>
emuflight · Oct 21, 2020 · 2c63a94 · 2c63a94
1 parent f96cbe1
commit 2c63a94
Show file tree

Hide file tree

Showing 5 changed files with 38 additions and 53 deletions.
diff --git a/src/board_comm/board_comm.c b/src/board_comm/board_comm.c
@@ -157,7 +157,7 @@ static void run_command(volatile imufCommand_t* command, volatile imufCommand_t*
                 filterConfig.sharpness            = (int16_t) ((int16_t)(command->param10 >> 16));
                 if (!filterConfig.sharpness)
                 {
-                	filterConfig.sharpness = 35;
+                	filterConfig.sharpness = 2500;
                 }
                 filterConfig.acc_lpf_hz          = (int16_t)(command->param10 & 0xFFFF);
                 if (!filterConfig.acc_lpf_hz)

diff --git a/src/filter/filter.c b/src/filter/filter.c
@@ -106,37 +106,9 @@ void filter_data(volatile axisData_t *gyroRateData, volatile axisData_t *gyroAcc
 	filteredData->rateData.y = biquad_update(filteredData->rateData.y, &(lpfFilterStateRate.y));
 	filteredData->rateData.z = biquad_update(filteredData->rateData.z, &(lpfFilterStateRate.z));
 
-// calculate the error
-	float errorMultiplierX = ABS(setPoint.x - filteredData->rateData.x) * sharpness;
-	float errorMultiplierY = ABS(setPoint.y - filteredData->rateData.y) * sharpness;
-	float errorMultiplierZ = ABS(setPoint.z - filteredData->rateData.z) * sharpness;
-
-// give a boost to the setpoint, used to caluclate the filter cutoff, based on the error and setpoint/gyrodata
-
-	errorMultiplierX = CONSTRAIN(errorMultiplierX * ABS(1.0f - (setPoint.x / filteredData->rateData.x)) + 1.0f, 1.0f, 10.0f);
-	errorMultiplierY = CONSTRAIN(errorMultiplierY * ABS(1.0f - (setPoint.y / filteredData->rateData.y)) + 1.0f, 1.0f, 10.0f);
-	errorMultiplierZ = CONSTRAIN(errorMultiplierZ * ABS(1.0f - (setPoint.z / filteredData->rateData.z)) + 1.0f, 1.0f, 10.0f);
-
-
 	if (setPointNew)
 	{
 		setPointNew = 0;
-		if (setPoint.x != 0.0f && oldSetPoint.x != setPoint.x)
-		{
-			filterConfig.roll_lpf_hz = CONSTRAIN((float)filterConfig.i_roll_lpf_hz * ABS(1.0f - ((setPoint.x * errorMultiplierX) / filteredData->rateData.x)), 10.0f, 500.0f);
-			filter_biquad_init(filterConfig.roll_lpf_hz, &(lpfFilterStateRate.x));
-		}
-		if (setPoint.y != 0.0f && oldSetPoint.y != setPoint.y)
-		{
-			filterConfig.pitch_lpf_hz = CONSTRAIN((float)filterConfig.i_pitch_lpf_hz * ABS(1.0f - ((setPoint.y * errorMultiplierY) / filteredData->rateData.y)), 10.0f, 500.0f);
-			filter_biquad_init(filterConfig.pitch_lpf_hz, &(lpfFilterStateRate.y));
-		}
-		if (setPoint.z != 0.0f && oldSetPoint.z != setPoint.z)
-		{
-			filterConfig.yaw_lpf_hz = CONSTRAIN((float)filterConfig.i_yaw_lpf_hz * ABS(1.0f - ((setPoint.z * errorMultiplierZ) / filteredData->rateData.z)), 10.0f, 500.0f);
-			filter_biquad_init(filterConfig.yaw_lpf_hz, &(lpfFilterStateRate.z));
-		}
-		memcpy((uint32_t *)&oldSetPoint, (uint32_t *)&setPoint, sizeof(axisData_t));
 	}
 	//no need to filter ACC is used in quaternions
 	filteredData->accData.x = gyroAccData->x;

diff --git a/src/filter/kalman.c b/src/filter/kalman.c
@@ -6,22 +6,23 @@
 variance_t varStruct;
 kalman_t   kalmanFilterStateRate[3];
 
-void init_kalman(kalman_t *filter, float q)
+void init_kalman(kalman_t *filter, float q, float sharpness)
 {
     memset(filter, 0, sizeof(kalman_t));
     filter->q = q * 0.001f;      //add multiplier to make tuning easier
     filter->r = 88.0f;           //seeding R at 88.0f
     filter->p = 30.0f;           //seeding P at 30.0f
     filter->e = 1.0f;
+    filter->s = sharpness * 0.01f;
 }
 
 void kalman_init(void)
 {
     setPointNew = 0;
     memset(&varStruct, 0, sizeof(varStruct));
-    init_kalman(&kalmanFilterStateRate[ROLL], filterConfig.roll_q);
-    init_kalman(&kalmanFilterStateRate[PITCH], filterConfig.pitch_q);
-    init_kalman(&kalmanFilterStateRate[YAW], filterConfig.yaw_q);
+    init_kalman(&kalmanFilterStateRate[ROLL], filterConfig.roll_q, filterConfig.sharpness);
+    init_kalman(&kalmanFilterStateRate[PITCH], filterConfig.pitch_q, filterConfig.sharpness);
+    init_kalman(&kalmanFilterStateRate[YAW], filterConfig.yaw_q, filterConfig.sharpness);
     varStruct.inverseN = 1.0f/filterConfig.w;
 }
 
@@ -85,28 +86,39 @@ void update_kalman_covariance(volatile axisData_t *gyroRateData)
 }
 
 inline float kalman_process(kalman_t* kalmanState, volatile float input, volatile float target) {
-    //project the state ahead using acceleration
-    kalmanState->x += (kalmanState->x - kalmanState->lastX);
-
-    //figure out how much to boost or reduce our error in the estimate based on setpoint target.
-    //this should be close to 0 as we approach the sepoint and really high the futher away we are from the setpoint.
-    //update last state
-    kalmanState->lastX = kalmanState->x;
-
-    if (target != 0.0f) {
-        kalmanState->e = ABS(1.0f - (target / kalmanState->lastX));
-    } else {
-        kalmanState->e = 1.0f;
+  //project the state ahead using acceleration
+  kalmanState->x += (kalmanState->x - kalmanState->lastX);
+
+  //figure out how much to boost or reduce our error in the estimate based on setpoint target.
+  //this should be close to 0 as we approach the sepoint and really high the futher away we are from the setpoint.
+  //update last state
+  kalmanState->lastX = kalmanState->x;
+
+  if (kalmanState->s != 0.0f) {
+    float average = fabsf(target + kalmanState->lastX) * 0.5f;
+
+    if (average > 10.0f)
+    {
+        float error = fabsf(target - kalmanState->lastX);
+        float ratio = error / average;
+        kalmanState->e = kalmanState->s * powf(ratio, 3.0f);  //"ratio" power 3 and multiply by a gain
     }
+    //prediction update
+    kalmanState->p = kalmanState->p + (kalmanState->q + kalmanState->e);
 
+  } else {
+    if (kalmanState->lastX != 0.0f)
+    {
+        kalmanState->e = fabsf(1.0f - (target / kalmanState->lastX));
+    }
     //prediction update
     kalmanState->p = kalmanState->p + (kalmanState->q * kalmanState->e);
-
-    //measurement update
-    kalmanState->k = kalmanState->p / (kalmanState->p + kalmanState->r);
-    kalmanState->x += kalmanState->k * (input - kalmanState->x);
-    kalmanState->p = (1.0f - kalmanState->k) * kalmanState->p;
-    return kalmanState->x;
+  }
+  //measurement update
+  kalmanState->k = kalmanState->p / (kalmanState->p + kalmanState->r);
+  kalmanState->x += kalmanState->k * (input - kalmanState->x);
+  kalmanState->p = (1.0f - kalmanState->k) * kalmanState->p;
+  return kalmanState->x;
 }
 
 void kalman_update(volatile axisData_t* input, filteredData_t* output)

diff --git a/src/filter/kalman.h b/src/filter/kalman.h
@@ -18,7 +18,8 @@ typedef struct kalman
     float k;     //kalman gain
     float x;     //state
     float lastX; //previous state
-    float e;
+    float e;     //multiplier or adder to q
+    float s;     //changes how dynamic e is
 } kalman_t;
 
 typedef struct variance

diff --git a/src/version.h b/src/version.h
@@ -3,4 +3,4 @@
 #define HARDWARE_VERSION 101
 #define BOOTLOADER_VERSION 101
 
-#define FIRMWARE_VERSION 231
+#define FIRMWARE_VERSION 250