An observer that can estimate the states from delayed signals
Fork of STATES_OBSERVER_DELAY by
STATES_OBSERVER_DELAY.h
00001 #ifndef STATES_OBSERVER_DELAY_H 00002 #define STATES_OBSERVER_DELAY_H 00003 // 00004 #include <vector> 00005 #include "CIRCULAR_BUFFER_VECTOR.h" 00006 00007 using std::vector; 00008 00009 /* 00010 // For debugging 00011 #include <iostream> 00012 using std::cout; 00013 // 00014 */ 00015 00016 //-----------------------------------------------------------------------// 00017 // Note: if the "delay_sample" was set to "0", the observer will be the regular observer 00018 //-----------------------------------------------------------------------// 00019 00020 00021 class STATES_OBSERVER_DELAY{ 00022 public: 00023 // Dimensions 00024 size_t n; // Number of states 00025 size_t p; // Number of inputs of the plant 00026 size_t q; // Number of outputs of the plant, no use in full-state-measureable case 00027 // 00028 size_t d; // Delay-sample in the measurements y 00029 00030 float Ts; // Sampling time 00031 // 00032 bool enable; 00033 bool flag_reset; 00034 00035 // Parameters for observer 00036 vector<vector<float> > Ad; // (Discrete) state-transition matrix of the plant 00037 vector<vector<float> > Bd; // (Discrete) Input matrix for u (the input matrix of the plant) 00038 // Cd: Output matric for the system, if necessary (the definition of the simulated states are different with the measurements) 00039 bool enable_Cd; // Decide if Cd is going to be used 00040 vector<vector<float> > Cd; 00041 // Gain matrix 00042 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) 00043 00044 // Input-signals of the observer 00045 vector<float> sys_inputs; // Input of the real system 00046 vector<float> sys_outputs; // Output of the real system (measurement, with d-sample delay) 00047 vector<float> sys_extraDisturbance; // "sys_extraDisturbance" is an n dimensional vector that interprets the summation of the extra inputs 00048 // Variables of the observer 00049 CIRCULAR_BUFFER_VECTOR states_est_buffer; // Estimated states from time k to time (k-d) 00050 vector<float> state_est; // Estimated states at time k (current) 00051 // vector<float> states_est_delay_d; // Estimated states at time k-d (delay d samples) 00052 vector<float> y_est; // Estimated output, Cd*states_est_delay_d (if necessary) 00053 vector<float> est_error; // Estimation error, est_error = y_est - sys_outputs 00054 00055 STATES_OBSERVER_DELAY(size_t num_state, size_t num_in, size_t num_out, size_t delay_sample, float samplingTime); 00056 // 00057 void start(); 00058 void stop(); 00059 void reset(); 00060 00061 // Assignments for observer 00062 //--------------------------------------------// 00063 // Assign continuous-time version of system matrices 00064 void assign_At(float* At_in, size_t n_in); // Continuous-time version 00065 void assign_Bt(float* Bt_in, size_t n_in, size_t p_in); // Continuous-time version 00066 void assign_Lt(float* Lt_in, size_t n_in, size_t d_in, size_t q_in); // Continuous-time version 00067 // ** Assign the continuous-time version of system matrices by matrices 00068 void assign_At(const vector<vector<float> > &At_in); 00069 void assign_Bt(const vector<vector<float> > &Bt_in); 00070 void assign_Lt(const vector<vector<float> > &Lt_in); 00071 // Assign discrete-time version of system matrices directly 00072 void assign_Ad(float* Ad_in, size_t n_in); // Discrete-time version 00073 void assign_Bd(float* Bd_in, size_t n_in, size_t p_in); // Discrete-time version 00074 //--------------------------------------------// 00075 void assign_Cd(float* Cd_in, size_t q_in, size_t n_in); 00076 // Assignment for observer Gain 00077 void assign_Ld(float* Ld_in, size_t n_in, size_t d_in, size_t q_in); 00078 00079 // Process of observer 00080 void iterateOnce(void); // Execute the simulator once 00081 00082 private: 00083 vector<float> zeros_n; 00084 vector<float> zeros_p; 00085 vector<float> zeros_q; 00086 00087 // Utilities 00088 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 00089 vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right 00090 vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b 00091 vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a 00092 // Increment 00093 void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a += (or -=) v_b 00094 // Partial matrix multiplication 00095 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 00096 }; 00097 00098 #endif
Generated on Tue Aug 2 2022 05:42:54 by
1.7.2
