Diff: SLIP_ACCELERATION.cpp

Revision:

3:265a69f4c360

Parent:

2:e275f3919042

Child:

4:7b8e13c54dfb

--- a/SLIP_ACCELERATION.cpp	Tue Jan 10 19:48:21 2017 +0000
+++ b/SLIP_ACCELERATION.cpp	Fri Jan 20 15:49:36 2017 +0000
@@ -1,18 +1,23 @@
 #include "SLIP_ACCELERATION.h"
 
-SLIP_ACCELERATION_2WHEEL::SLIP_ACCELERATION_2WHEEL(size_t onBoardDelay_ms_in, float samplingTime):
+SLIP_ACCELERATION_2WHEEL::SLIP_ACCELERATION_2WHEEL(size_t onBoardDelay_ms_in, size_t wheelDelay_ms_in, float samplingTime):
         Ts(samplingTime),
         onBoardDelay_ms(onBoardDelay_ms_in),
+        wheelDelay_ms(wheelDelay_ms_in),
         yawAcce_cal(samplingTime),
-        lpf_dVs(2,samplingTime,200), // 200 Hz
-        hpf_dVs(2,samplingTime,0.1), // 0.015 Hz
-        hpf_dVs_2ndLayer(2,samplingTime,0.1), // The 2nd layer, 0.015 Hz
-        OnboardSignal_FIFO() // Empty
+        lpf_dVs(2,samplingTime,400.0), // 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
+        OnboardSignal_FIFO(), // Empty
+        wheelSignal_FIFO() // Empty
 {
     //
     n = 2; // This class is made specifically for differential-drive vehicle
     //
     zeros_2.assign(2,0.0);
+    // Not initialized
+    Flag_Init = false;
+    init_count = 0;
 
     // Parameters
     btotal = 0.194; // 0.2; // m
@@ -30,41 +35,89 @@
     yawAcce = 0.0;
     acce_wheelCenter = zeros_2;
     acce_wheelCenter_delay = zeros_2;
+    acce_wheelSpeed_delay = zeros_2;
 
     // Results
     dVs = zeros_2;
+    dVs_filtered = zeros_2;
+    //
+    dVs_unbiased = zeros_2;
+    dVs_bias = zeros_2;
 
 
     // Initialize the queue
     for (size_t i = 0; i < onBoardDelay_ms; ++i){
         OnboardSignal_FIFO.push(zeros_2);
     }
+    // Initialize the queue
+    for (size_t i = 0; i < onBoardDelay_ms; ++i){
+        wheelSignal_FIFO.push(zeros_2);
+    }
 }
 vector<float> SLIP_ACCELERATION_2WHEEL::iterateOnce(void){ // Calculate dVs
 
+    // Initialization
+    if (!Flag_Init){
+        if (init_count <= (max(wheelDelay_ms, onBoardDelay_ms)+1) ){
+            dVs_filtered = zeros_2;
+            init_count++;
+            return dVs_filtered;
+        }
+        dVs_bias = zeros_2;
+        dVs_unbiased = dVs;
+        dVs_filtered = zeros_2;
+        //
+        resetFilter();
+        Flag_Init = true;
+        return dVs_filtered; // output = 0.0
+    }
 
     // Claculate the latest signal
     acce_wheelCenter_cal();
 
     // Pushing queue (On-board signals need to be delayed)
-    queueOp(acce_wheelCenter_delay,acce_wheelCenter);
+    // queueOp(acce_wheelCenter_delay,acce_wheelCenter);
+    queueOp(acce_wheelCenter_delay, acce_wheelCenter, OnboardSignal_FIFO, onBoardDelay_ms);
+    queueOp(acce_wheelSpeed_delay, acce_wheelSpeed, wheelSignal_FIFO, wheelDelay_ms);
 
     // Calculate the dVs
+    /*
     for (size_t i = 0; i < n; ++i){
         // acce_wheelSpeed[i] = r*wheelRotationalAcce[i];
-        dVs[i] = acce_wheelSpeed[i] - acce_wheelCenter_delay[i];
+        // dVs[i] = acce_wheelSpeed[i] - acce_wheelCenter_delay[i];
+        dVs[i] = acce_wheelSpeed_delay[i] - acce_wheelCenter_delay[i];
     }
+    */
+
+    // dVs = acce_wheelSpeed_delay - acce_wheelCenter_delay;
+    dVs = Get_VectorPlus(acce_wheelSpeed_delay, acce_wheelCenter_delay, true); // minus
+
 
     // Filtering the dVs
     // dVs_filtered = dVs; // Nothing to do
     // dVs_filtered = lpf_dVs.filter(dVs); // Low-pass: 0.0 Hz ~ 200 Hz
+    //
     // dVs_filtered = hpf_dVs.filter(lpf_dVs.filter(dVs)); // Band-pass: 0.015 Hz ~ 200 Hz
-    dVs_filtered = hpf_dVs_2ndLayer.filter( 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
 
     //
     // return dVs;
     return dVs_filtered;
 }
+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); // +=
+    //
+    yawAcce_cal.reset(yawRate);
+    lpf_dVs.reset(zeros_2);
+    hpf_dVs.reset(zeros_2);
+    dVs_filtered = zeros_2;
+}
 //
 void SLIP_ACCELERATION_2WHEEL::acce_wheelCenter_cal(void){ // Calculate the linear acceleration at each wheel center
     // Calculate the yawAcce
@@ -75,7 +128,7 @@
     // acce_wheelCenter[1] = acce_vehicleCenter; // Left
     //
     acce_wheelCenter[0] = acce_vehicleCenter + yawAcce*br; // Right
-    acce_wheelCenter[1] = acce_vehicleCenter - yawAcce*br; // Left
+    acce_wheelCenter[1] = acce_vehicleCenter - yawAcce*bl; // Left
 }
 //
 void SLIP_ACCELERATION_2WHEEL::queueOp(vector<float> &signal_delayed, const vector<float> &signal_in){ // The operation of the queue
@@ -100,3 +153,139 @@
         // Now: OnboardSignal_FIFO.size() = onBoardDelay_ms
     }
 }
+void SLIP_ACCELERATION_2WHEEL::queueOp(vector<float> &signal_delayed, const vector<float> &signal_in, queue<vector<float> > &queue_in, size_t queue_length){ // The operation of the queue
+
+    // Push into queue first to prevent the "empty" error when queue_length is zero
+    queue_in.push(signal_in);
+
+    //----------------------------------------------------------//
+
+    // Get the delayed signal: signal_delayed
+    signal_delayed = queue_in.front();
+
+    // Control the buffer size
+    // Currently, the size of the queue is expected to be (queue_length + 1)
+    if (queue_in.size() <= queue_length){
+        // nothing to do
+    }else{
+        //
+        while(queue_in.size() > queue_length){
+            queue_in.pop();
+        }
+        // Now: queue_in.size() = queue_length
+    }
+}
+// Utilities
+void SLIP_ACCELERATION_2WHEEL::Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
+    static vector<float>::iterator it_out;
+    static vector<const float>::iterator it_m_row;
+    static vector<const float>::iterator it_v;
+    //
+    it_out = v_out.begin();
+    for (size_t i = 0; i < m_left.size(); ++i){
+        *it_out = 0.0;
+        it_m_row = m_left[i].begin();
+        it_v = v_right.begin();
+        for (size_t j = 0; j < m_left[i].size(); ++j){
+            // *it_out += m_left[i][j] * v_right[j];
+            if (*it_m_row != 0.0 && *it_v != 0.0){
+                (*it_out) += (*it_m_row) * (*it_v);
+            }else{
+                // (*it_out) += 0.0
+            }
+            // (*it_out) += (*it_m_row) * (*it_v);
+            //
+            it_m_row++;
+            it_v++;
+        }
+        it_out++;
+    }
+}
+vector<float> SLIP_ACCELERATION_2WHEEL::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
+    static vector<float> v_out;
+    // Size check
+    if (v_out.size() != m_left.size()){
+        v_out.resize(m_left.size());
+    }
+    // Iterators
+    static vector<float>::iterator it_out;
+    static vector<const float>::iterator it_m_row;
+    static vector<const float>::iterator it_v;
+    //
+    it_out = v_out.begin();
+    for (size_t i = 0; i < m_left.size(); ++i){
+        *it_out = 0.0;
+        it_m_row = m_left[i].begin();
+        it_v = v_right.begin();
+        for (size_t j = 0; j < m_left[i].size(); ++j){
+            // *it_out += m_left[i][j] * v_right[j];
+            if (*it_m_row != 0.0 && *it_v != 0.0){
+                (*it_out) += (*it_m_row) * (*it_v);
+            }else{
+                // (*it_out) += 0.0
+            }
+            // (*it_out) += (*it_m_row) * (*it_v);
+            //
+            it_m_row++;
+            it_v++;
+        }
+        it_out++;
+    }
+    return v_out;
+}
+vector<float> SLIP_ACCELERATION_2WHEEL::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
+{
+    static vector<float> v_c;
+    // Size check
+    if (v_c.size() != v_a.size()){
+        v_c.resize(v_a.size());
+    }
+    //
+    for (size_t i = 0; i < v_a.size(); ++i){
+        if (is_minus){
+            v_c[i] = v_a[i] - v_b[i];
+        }else{
+            v_c[i] = v_a[i] + v_b[i];
+        }
+    }
+    return v_c;
+}
+vector<float> SLIP_ACCELERATION_2WHEEL::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
+{
+    static vector<float> v_c;
+    // Size check
+    if (v_c.size() != v_a.size()){
+        v_c.resize(v_a.size());
+    }
+    // for pure negative
+    if (scale == -1.0){
+        for (size_t i = 0; i < v_a.size(); ++i){
+            v_c[i] = -v_a[i];
+        }
+        return v_c;
+    }
+    // else
+    for (size_t i = 0; i < v_a.size(); ++i){
+        v_c[i] = scale*v_a[i];
+
+    }
+    return v_c;
+}
+// Increment
+void SLIP_ACCELERATION_2WHEEL::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
+    // Size check
+    if (v_a.size() != v_b.size()){
+        v_a.resize(v_b.size());
+    }
+    //
+    if (is_minus){ // -=
+        for (size_t i = 0; i < v_b.size(); ++i){
+            v_a[i] -= v_b[i];
+        }
+    }else{ // +=
+        for (size_t i = 0; i < v_b.size(); ++i){
+            v_a[i] += v_b[i];
+        }
+    }
+
+}