File content as of revision 1:085d41355949:

#ifndef STATES_OBSERVER_DELAY_H
#define STATES_OBSERVER_DELAY_H
//
#include <vector>
#include "CIRCULAR_BUFFER_VECTOR.h"

using std::vector;

/*
// For debugging
#include <iostream>
using std::cout;
//
*/

//-----------------------------------------------------------------------//
// Note: if the "delay_sample" was set to "0", the observer will be the regular observer
//-----------------------------------------------------------------------//


class STATES_OBSERVER_DELAY{
public:
    // Dimensions
    size_t n; // Number of states
    size_t p; // Number of inputs of the plant
    size_t q; // Number of outputs of the plant, no use in full-state-measureable case
    //
    size_t d; // Delay-sample in the measurements y

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

    // Parameters for observer
    vector<vector<float> > Ad; // (Discrete) state-transition matrix of the plant
    vector<vector<float> > Bd; // (Discrete) Input matrix for u (the input matrix of the plant)
    // Cd: Output matric for the system, if necessary (the definition of the simulated states are different with the measurements)
    bool enable_Cd; // Decide if Cd is going to be used
    vector<vector<float> > Cd;
    // Gain matrix
    vector<vector<float> > Ld; // Gain matrix for the observer with measurement-delay, Ld is an n(d+1) by q matrix (or n(d+1) by n if Cd is not used)

    // Input-signals of the observer
    vector<float> sys_inputs; // Input of the real system
    vector<float> sys_outputs; // Output of the real system (measurement, with d-sample delay)
    vector<float> sys_extraDisturbance; // "sys_extraDisturbance" is an n dimensional vector that interprets the summation of the extra inputs
    // Variables of the observer
    CIRCULAR_BUFFER_VECTOR states_est_buffer; // Estimated states from time k to time (k-d)
    vector<float> state_est; // Estimated states at time k (current)
    // vector<float> states_est_delay_d; // Estimated states at time k-d (delay d samples)
    vector<float> y_est; // Estimated output, Cd*states_est_delay_d (if necessary)
    vector<float> est_error; // Estimation error, est_error = y_est - sys_outputs

    STATES_OBSERVER_DELAY(size_t num_state, size_t num_in, size_t num_out, size_t delay_sample, float samplingTime);
    //
    void start();
    void stop();
    void reset();

    // Assignments for observer
    //--------------------------------------------//
    // Assign continuous-time version of system matrices
    void assign_At(float* At_in, size_t n_in); // Continuous-time version
    void assign_Bt(float* Bt_in, size_t n_in, size_t p_in); // Continuous-time version
    void assign_Lt(float* Lt_in, size_t n_in, size_t d_in, size_t q_in); // Continuous-time version
    // ** Assign the continuous-time version of system matrices by matrices
    void assign_At(const vector<vector<float> > &At_in);
    void assign_Bt(const vector<vector<float> > &Bt_in);
    void assign_Lt(const vector<vector<float> > &Lt_in);
    // Assign discrete-time version of system matrices directly
    void assign_Ad(float* Ad_in, size_t n_in); // Discrete-time version
    void assign_Bd(float* Bd_in, size_t n_in, size_t p_in); // Discrete-time version
    //--------------------------------------------//
    void assign_Cd(float* Cd_in, size_t q_in, size_t n_in);
    // Assignment for observer Gain
    void assign_Ld(float* Ld_in, size_t n_in, size_t d_in, size_t q_in);

    // Process of observer
    void iterateOnce(void); // Execute the simulator once

private:
    vector<float> zeros_n;
    vector<float> zeros_p;
    vector<float> zeros_q;

    // 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
    // Partial matrix multiplication
    vector<float> Partial_Mat_multiply_Vec(vector<vector<float> >::iterator &it_m_left, size_t numRow_m_left, const vector<float> &v_right); // v_out = m_left*v_right
};

#endif