Chun Feng Huang / SLIP_ACCELERATION

The library for calculating and processing the slip acceleration

Files at this revision

API Documentation at this revision

Revision 4:7b8e13c54dfb, committed 2017-02-07

Comitter:
benson516
Date:
Tue Feb 07 09:54:15 2017 +0000
Parent:
3:265a69f4c360
Commit message:
test succeed

Changed in this revision

SLIP_ACCELERATION.cpp Show annotated file Show diff for this revision Revisions of this file
SLIP_ACCELERATION.h Show annotated file Show diff for this revision Revisions of this file 
--- a/SLIP_ACCELERATION.cpp	Fri Jan 20 15:49:36 2017 +0000
+++ b/SLIP_ACCELERATION.cpp	Tue Feb 07 09:54:15 2017 +0000
@@ -5,9 +5,10 @@
         onBoardDelay_ms(onBoardDelay_ms_in),
         wheelDelay_ms(wheelDelay_ms_in),
         yawAcce_cal(samplingTime),
-        lpf_dVs(2,samplingTime,400.0), // 200 Hz
+        lpf_dVs(2, samplingTime, 300.0, 1), // 200 Hz
         // hpf_dVs(2,samplingTime, 0.01, 4), // 0.1 Hz, 4th order critical-damped HPF
-        hpf_dVs(2,samplingTime, 0.05, 1), // 0.1 Hz, 4th order critical-damped HPF
+        hpf_dVs(2,samplingTime, 0.01, 1), // 0.1 Hz, 4th order critical-damped HPF
+        lpf_dVs_biasedEst(2,samplingTime, 0.5, 2), // 0.5 Hz
         OnboardSignal_FIFO(), // Empty
         wheelSignal_FIFO() // Empty
 {
@@ -99,10 +100,21 @@
     //
     // dVs_filtered = hpf_dVs.filter(lpf_dVs.filter(dVs)); // Band-pass: 0.015 Hz ~ 200 Hz
 
+
+
     // dVs_unbiased = dVs - dVs_bias
     dVs_unbiased = Get_VectorPlus(dVs, dVs_bias, true); // minus
+    dVs_filtered = hpf_dVs.filter(lpf_dVs.filter(dVs_unbiased)); // Band-pass: 0.015 Hz ~ 200 Hz
 
-    dVs_filtered = hpf_dVs.filter(lpf_dVs.filter(dVs_unbiased)); // Band-pass: 0.015 Hz ~ 200 Hz
+
+    /*
+    // dVs_unbiased = dVs - dVs_bias
+    dVs_unbiased = lpf_dVs.filter( Get_VectorPlus(dVs, dVs_bias, true) ); // minus
+    dVs_filtered = hpf_dVs.filter(dVs_unbiased); // Band-pass: 0.015 Hz ~ 200 Hz
+    */
+
+    lpf_dVs_biasedEst.filter(dVs); // Get the estimation of the bias in dVs
+
 
     //
     // return dVs;
@@ -111,7 +123,12 @@
 void SLIP_ACCELERATION_2WHEEL::resetFilter(void){ // Rest all filters, includeing LPF and HPF
     // Bias calculation
     // dVs_bias += dVs_unbiased - dVs_filtered
-    Get_VectorIncrement(dVs_bias, Get_VectorPlus(dVs_unbiased, dVs_filtered, true), false); // +=
+    // Get_VectorIncrement(dVs_bias, Get_VectorPlus(dVs_unbiased, dVs_filtered, true), false); // +=
+
+    // Set the bias as the estimation of bias
+    dVs_bias = lpf_dVs_biasedEst.output;
+    dVs_unbiased = zeros_2;
+
     //
     yawAcce_cal.reset(yawRate);
     lpf_dVs.reset(zeros_2);

--- a/SLIP_ACCELERATION.h	Fri Jan 20 15:49:36 2017 +0000
+++ b/SLIP_ACCELERATION.h	Tue Feb 07 09:54:15 2017 +0000
@@ -61,11 +61,13 @@
 
     // Filters
     Derivative_appr yawAcce_cal; // Calculate the yawAcce
-    LPF_vector lpf_dVs; // Low-pass filter for dVs
+    LPF_vector_nthOrderCritical lpf_dVs; // Low-pass filter for dVs
     //
     HPF_vector_nthOrderCritical hpf_dVs; // nth-order critical-damped High-pass filter for dVs
     // HPF_vector_1minusLPF_nthOrderCritical hpf_dVs; // nth-order critical-damped High-pass filter for dVs
 
+    // Estimation of the bias
+    LPF_vector_nthOrderCritical lpf_dVs_biasedEst; // Low-pass filter for dVs to get the estimation of bias quickly
 
     // Queue for delay
     queue<vector<float> > OnboardSignal_FIFO;