File content as of revision 4:7b8e13c54dfb:

#include "SLIP_ACCELERATION.h"

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, 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.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
{
    //
    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
    br = 0.103; // btotal/2.0; // m
    bl = btotal - br; // m
    r = 0.139/2.0; // m

    // Inputs
    acce_vehicleCenter = 0.0;
    // wheelRotationalAcce = zeros_2;
    acce_wheelSpeed = zeros_2;
    yawRate = 0.0;

    // States
    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, 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_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_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_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;
    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); // +=

    // 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);
    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
    yawAcce = yawAcce_cal.filter(yawRate);

    // Calculate the linear acceleration at each wheel center
    // acce_wheelCenter[0] = acce_vehicleCenter; // Right
    // acce_wheelCenter[1] = acce_vehicleCenter; // Left
    //
    acce_wheelCenter[0] = acce_vehicleCenter + yawAcce*br; // Right
    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

    // Push into queue first to prevent the "empty" error when onBoardDelay_ms is zero
    OnboardSignal_FIFO.push(signal_in);

    //----------------------------------------------------------//

    // Get the delayed signal: signal_delayed
    signal_delayed = OnboardSignal_FIFO.front();

    // Control the buffer size
    // Currently, the size of the queue is expected to be (onBoardDelay_ms + 1)
    if (OnboardSignal_FIFO.size() <= onBoardDelay_ms){
        // nothing to do
    }else{
        //
        while(OnboardSignal_FIFO.size() > onBoardDelay_ms){
            OnboardSignal_FIFO.pop();
        }
        // 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];
        }
    }

}