Diff: SLIP_ACCELERATION.h

Revision:

3:265a69f4c360

Parent:

2:e275f3919042

Child:

4:7b8e13c54dfb

--- a/SLIP_ACCELERATION.h	Tue Jan 10 19:48:21 2017 +0000
+++ b/SLIP_ACCELERATION.h	Fri Jan 20 15:49:36 2017 +0000
@@ -3,17 +3,20 @@
 //
 #include <vector>
 #include <queue>
+#include <algorithm>
 #include "FILTER_LIB.h"
 
 //
 using std::vector;
 using std::queue;
+using std::max;
 
 class SLIP_ACCELERATION_2WHEEL{
 public:
     size_t n; // Number of wheels
     float Ts;
     size_t onBoardDelay_ms; // Note that this is allowed to be zero, which means no delay
+    size_t wheelDelay_ms; // Note that this is allowed to be zero, which means no delay
 
     // Parameters
     float btotal; // m, The distance between left wheel and right wheel
@@ -31,32 +34,55 @@
     float yawAcce;
     vector<float> acce_wheelCenter; // 0-> R, 1-> L
     vector<float> acce_wheelCenter_delay; // Delay onBoardDelay_ms
+    //
+    vector<float> acce_wheelSpeed_delay; // Delay wheelDelay_ms
+    //
+    vector<float> dVs_unbiased; // dVs_unbiased = dVs - dVs_bias;
+    vector<float> dVs_bias; // The bias in dVs
+
 
     // Results
     vector<float> dVs; // Raw signal, dVs[0]: Right, dVs[1]: Left
     vector<float> dVs_filtered; // Filtered version of dVs
 
+
     //
-    SLIP_ACCELERATION_2WHEEL(size_t onBoardDelay_ms_in, float samplingTime);
+    SLIP_ACCELERATION_2WHEEL(size_t onBoardDelay_ms_in, size_t wheelDelay_ms_in, float samplingTime);
     vector<float> iterateOnce(void); // Calculate dVs
+    void resetFilter(void); // Rest all filters, includeing LPF and HPF
 
 private:
     //
     vector<float> zeros_2;
 
+    // Flag
+    bool Flag_Init;
+    size_t init_count;
+
     // Filters
     Derivative_appr yawAcce_cal; // Calculate the yawAcce
     LPF_vector lpf_dVs; // Low-pass filter for dVs
-    HPF_vector hpf_dVs; // High-pass filter for dVs
     //
-    HPF_vector hpf_dVs_2ndLayer; // High-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
+
 
     // Queue for delay
     queue<vector<float> > OnboardSignal_FIFO;
+    queue<vector<float> > wheelSignal_FIFO;
 
     //
     void acce_wheelCenter_cal(void); // Calculate the linear acceleration at each wheel center
     void queueOp(vector<float> &signal_delayed, const vector<float> &signal_in); // The operation of the queue
+    void queueOp(vector<float> &signal_delayed, const vector<float> &signal_in, queue<vector<float> > &queue_in, size_t queue_length);
+
+    // Utilities
+    void Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right
+    vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right
+    vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b
+    vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
+    // Increment
+    void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a += (or -=) v_b
 };
 
 #endif