An observer that can estimate the states from delayed signals

Fork of STATES_OBSERVER_DELAY by Project_WIPV_antiSlip

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers STATES_OBSERVER_DELAY.h Source File

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