Project_WIPV_antiSlip / STATES_OBSERVER_DELAY

An observer that can estimate the states from delayed signals

Fork of STATES_OBSERVER_DELAY by Project_WIPV_antiSlip

STATES_OBSERVER_DELAY.h@1:085d41355949, 2017-02-10 (annotated)

Committer:
benson516
Date:
Fri Feb 10 18:27:14 2017 +0000
Revision:
1:085d41355949
Parent:
0:0699f8e638ca 
Fix some serious bugs

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benson516 0:0699f8e638ca 1 #ifndef STATES_OBSERVER_DELAY_H
benson516 0:0699f8e638ca 2 #define STATES_OBSERVER_DELAY_H
benson516 0:0699f8e638ca 3 //
benson516 0:0699f8e638ca 4 #include <vector>
benson516 0:0699f8e638ca 5 #include "CIRCULAR_BUFFER_VECTOR.h"
benson516 0:0699f8e638ca 6
benson516 0:0699f8e638ca 7 using std::vector;
benson516 0:0699f8e638ca 8
benson516 1:085d41355949 9 /*
benson516 1:085d41355949 10 // For debugging
benson516 1:085d41355949 11 #include <iostream>
benson516 1:085d41355949 12 using std::cout;
benson516 1:085d41355949 13 //
benson516 1:085d41355949 14 */
benson516 0:0699f8e638ca 15
benson516 0:0699f8e638ca 16 //-----------------------------------------------------------------------//
benson516 0:0699f8e638ca 17 // Note: if the "delay_sample" was set to "0", the observer will be the regular observer
benson516 0:0699f8e638ca 18 //-----------------------------------------------------------------------//
benson516 0:0699f8e638ca 19
benson516 0:0699f8e638ca 20
benson516 0:0699f8e638ca 21 class STATES_OBSERVER_DELAY{
benson516 0:0699f8e638ca 22 public:
benson516 0:0699f8e638ca 23 // Dimensions
benson516 0:0699f8e638ca 24 size_t n; // Number of states
benson516 0:0699f8e638ca 25 size_t p; // Number of inputs of the plant
benson516 0:0699f8e638ca 26 size_t q; // Number of outputs of the plant, no use in full-state-measureable case
benson516 0:0699f8e638ca 27 //
benson516 0:0699f8e638ca 28 size_t d; // Delay-sample in the measurements y
benson516 0:0699f8e638ca 29
benson516 0:0699f8e638ca 30 float Ts; // Sampling time
benson516 0:0699f8e638ca 31 //
benson516 0:0699f8e638ca 32 bool enable;
benson516 0:0699f8e638ca 33 bool flag_reset;
benson516 0:0699f8e638ca 34
benson516 0:0699f8e638ca 35 // Parameters for observer
benson516 0:0699f8e638ca 36 vector<vector<float> > Ad; // (Discrete) state-transition matrix of the plant
benson516 0:0699f8e638ca 37 vector<vector<float> > Bd; // (Discrete) Input matrix for u (the input matrix of the plant)
benson516 0:0699f8e638ca 38 // Cd: Output matric for the system, if necessary (the definition of the simulated states are different with the measurements)
benson516 0:0699f8e638ca 39 bool enable_Cd; // Decide if Cd is going to be used
benson516 0:0699f8e638ca 40 vector<vector<float> > Cd;
benson516 0:0699f8e638ca 41 // Gain matrix
benson516 0:0699f8e638ca 42 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)
benson516 0:0699f8e638ca 43
benson516 0:0699f8e638ca 44 // Input-signals of the observer
benson516 0:0699f8e638ca 45 vector<float> sys_inputs; // Input of the real system
benson516 0:0699f8e638ca 46 vector<float> sys_outputs; // Output of the real system (measurement, with d-sample delay)
benson516 0:0699f8e638ca 47 vector<float> sys_extraDisturbance; // "sys_extraDisturbance" is an n dimensional vector that interprets the summation of the extra inputs
benson516 0:0699f8e638ca 48 // Variables of the observer
benson516 0:0699f8e638ca 49 CIRCULAR_BUFFER_VECTOR states_est_buffer; // Estimated states from time k to time (k-d)
benson516 0:0699f8e638ca 50 vector<float> state_est; // Estimated states at time k (current)
benson516 0:0699f8e638ca 51 // vector<float> states_est_delay_d; // Estimated states at time k-d (delay d samples)
benson516 0:0699f8e638ca 52 vector<float> y_est; // Estimated output, Cd*states_est_delay_d (if necessary)
benson516 0:0699f8e638ca 53 vector<float> est_error; // Estimation error, est_error = y_est - sys_outputs
benson516 0:0699f8e638ca 54
benson516 0:0699f8e638ca 55 STATES_OBSERVER_DELAY(size_t num_state, size_t num_in, size_t num_out, size_t delay_sample, float samplingTime);
benson516 0:0699f8e638ca 56 //
benson516 0:0699f8e638ca 57 void start();
benson516 0:0699f8e638ca 58 void stop();
benson516 0:0699f8e638ca 59 void reset();
benson516 0:0699f8e638ca 60
benson516 0:0699f8e638ca 61 // Assignments for observer
benson516 0:0699f8e638ca 62 //--------------------------------------------//
benson516 0:0699f8e638ca 63 // Assign continuous-time version of system matrices
benson516 0:0699f8e638ca 64 void assign_At(float* At_in, size_t n_in); // Continuous-time version
benson516 0:0699f8e638ca 65 void assign_Bt(float* Bt_in, size_t n_in, size_t p_in); // Continuous-time version
benson516 1:085d41355949 66 void assign_Lt(float* Lt_in, size_t n_in, size_t d_in, size_t q_in); // Continuous-time version
benson516 0:0699f8e638ca 67 // ** Assign the continuous-time version of system matrices by matrices
benson516 0:0699f8e638ca 68 void assign_At(const vector<vector<float> > &At_in);
benson516 0:0699f8e638ca 69 void assign_Bt(const vector<vector<float> > &Bt_in);
benson516 1:085d41355949 70 void assign_Lt(const vector<vector<float> > &Lt_in);
benson516 0:0699f8e638ca 71 // Assign discrete-time version of system matrices directly
benson516 0:0699f8e638ca 72 void assign_Ad(float* Ad_in, size_t n_in); // Discrete-time version
benson516 0:0699f8e638ca 73 void assign_Bd(float* Bd_in, size_t n_in, size_t p_in); // Discrete-time version
benson516 0:0699f8e638ca 74 //--------------------------------------------//
benson516 0:0699f8e638ca 75 void assign_Cd(float* Cd_in, size_t q_in, size_t n_in);
benson516 0:0699f8e638ca 76 // Assignment for observer Gain
benson516 0:0699f8e638ca 77 void assign_Ld(float* Ld_in, size_t n_in, size_t d_in, size_t q_in);
benson516 0:0699f8e638ca 78
benson516 0:0699f8e638ca 79 // Process of observer
benson516 0:0699f8e638ca 80 void iterateOnce(void); // Execute the simulator once
benson516 0:0699f8e638ca 81
benson516 0:0699f8e638ca 82 private:
benson516 0:0699f8e638ca 83 vector<float> zeros_n;
benson516 0:0699f8e638ca 84 vector<float> zeros_p;
benson516 0:0699f8e638ca 85 vector<float> zeros_q;
benson516 0:0699f8e638ca 86
benson516 0:0699f8e638ca 87 // Utilities
benson516 0:0699f8e638ca 88 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
benson516 0:0699f8e638ca 89 vector<float> Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right); // v_out = m_left*v_right
benson516 0:0699f8e638ca 90 vector<float> Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a + (or -) v_b
benson516 0:0699f8e638ca 91 vector<float> Get_VectorScalarMultiply(const vector<float> &v_a, float scale); // scale*v_a
benson516 0:0699f8e638ca 92 // Increment
benson516 0:0699f8e638ca 93 void Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus); // v_a += (or -=) v_b
benson516 0:0699f8e638ca 94 // Partial matrix multiplication
benson516 0:0699f8e638ca 95 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
benson516 0:0699f8e638ca 96 };
benson516 0:0699f8e638ca 97
benson516 0:0699f8e638ca 98 #endif