File content as of revision 1:fbae330f0e71:

#ifndef Observer_ftRatio_H
#define Observer_ftRatio_H
//
#include "FILTER_LIB.h"
// #include "STATES_TRANSFORM.h"
#include "STATES_OBSERVER_DELAY.h"
#include <vector>
//
using std::vector;

// #define NUM_THRESHOLD 0.001 // 10^3



class Observer_ftRatio{
public:
    // Dimensions of the original system
    size_t n; // Number of states
    size_t p; // Number of inputs of the plant, the dimension of y
    size_t q; // Number of outputs of the plant, no use in full-state-measureable case
    // Dimensions of the observer
    size_t n_OB; // Number of states
    size_t p_OB; // Number of inputs of the plant, the dimension of y
    size_t q_OB; // Number of outputs of the plant, no use in full-state-measureable case
    size_t n_ME_OB; // Number of the measurement errors in the PI observer

    float Ts; // Sampling time
    //
    bool enable;

    //
    STATES_OBSERVER_DELAY OB;

    // Parameters
    // vector<vector<float> > Bv1_ftOb;
    // Parameters for calculating ft_ideal
    vector<vector<float> > F1_ftOb;
    vector<vector<float> > F2_ftOb;

    // Indexes
    // size_t idx_x_real; // Index for x_real
    size_t idx_x_ns; // Index for x_ns
    size_t idx_ft_error_est; // Index for ft_error_est

    // Input signals
    vector<float> yc; // THe outputs of the nominal controller
    // States
    vector<float> ft_est;
    vector<float> ft_error_est;
    vector<float> ft_ideal_est;
    //
    vector<float> x_real_est; // Estimation of the real states
    // vector<float> x_ns_est; // Estimation of the states of the no-slip system

    //
    vector<float> Measurement_error_est; // The estimation of measurement errors in PI observer

    // The degree of no-slip
    vector<float> ratio_ft;
    vector<float> ratio_ft_filtered;


    Observer_ftRatio(float samplingTime);
    //
    void start();
    void pause();
    void stop();
    void reset();
    //
    void init(void);
    // Process of observer
    void iterateOnce(const vector<float> &yc_in, const vector<float> &x_real_in); // Execute the simulator once

private:
    vector<float> zeros_n;
    vector<float> zeros_p;
    vector<float> zeros_q;
    vector<float> zeros_n_ME_OB; // Number of the measurement errors in the PI observer
    //
    vector<float> ones_p;

    // Private methods
    //
    void insertSignals(const vector<float> &yc_in, const vector<float> &x_real_in);



    // Kalman-filter-based parameter identifier for ratio-ft(s)
    // TRANSFORM_MOTOR_separate_2_avgDelta T_ft_est, T_ft_ideal_est;
    FirstOrder_KalmanFilter KFID_ratio_ft_right;
    FirstOrder_KalmanFilter KFID_ratio_ft_left;
    // Filter for ratio_ft
    LPF_vector_nthOrderCritical LPF_ratio_ft;

    // Saturation
    Saturation_vector SAT_ratio_ft;

    // 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


    // Assign a matrix by primitive c-array
    void assign_Matrix(vector<vector<float> > &M, float* M_in, size_t m_in, size_t n_in); // M_in is a m_in by n_in array

};

#endif