Chun Feng Huang
An observer for estimating the friction-force ratio
Diff: Observer_ftRatio.cpp
- Revision:
- 0:a936477fcd4a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Observer_ftRatio.cpp Thu Feb 23 11:12:06 2017 +0000
@@ -0,0 +1,442 @@
+#include "Observer_ftRatio.h"
+
+//
+float Dimension_ftOb[] = {
+ #include "Dimensions_ftOb.txt" // Load the parameter Dimensions_ftOb
+};
+//
+
+Observer_ftRatio::Observer_ftRatio(float samplingTime):
+ Ts(samplingTime),
+ OB(Dimension_ftOb[3], Dimension_ftOb[4], Dimension_ftOb[5], 0, samplingTime),
+ KFID_ratio_ft_right(samplingTime, 1.0, 0.0, 1.0, 1.0, 1.0, false), // For identifing the ratio_ft of the right wheel
+ KFID_ratio_ft_left(samplingTime, 1.0, 0.0, 1.0, 1.0, 1.0, false), // For identifing the ratio_ft of the left wheel
+ SAT_ratio_ft(Dimension_ftOb[1], 1.0, 0.0),
+ LPF_ratio_ft(Dimension_ftOb[1], samplingTime, 10.0, 1) // fc = 10 Hz
+{
+ // To enble, run *.start() function
+ enable = false;
+ // Dimensions of the original system
+ n = Dimension_ftOb[0];
+ p = Dimension_ftOb[1];
+ q = Dimension_ftOb[2];
+ // Dimensions of the observer
+ n_OB = Dimension_ftOb[3];
+ p_OB = Dimension_ftOb[4];
+ q_OB = Dimension_ftOb[5];
+ n_ME_OB = Dimension_ftOb[6]; // Number of the measurement errors in the PI observer
+ //
+ zeros_n.assign(n, 0.0);
+ zeros_p.assign(p, 0.0);
+ zeros_q.assign(q, 0.0);
+ zeros_n_ME_OB.assign(n_ME_OB, 0.0);
+ //
+ ones_p.assign(p, 1.0);
+
+ // Assign parameters
+ // init();
+
+ // Indexes
+ // idx_x_real = 0; // Index for x_real_est
+ idx_x_ns = n;
+ idx_ft_error_est = 2*n; // Index for ft_error_est
+
+ // Input signals
+ yc = zeros_p;
+ // States
+ ft_est = zeros_p;
+ ft_error_est = zeros_p;
+ ft_ideal_est = zeros_p;
+ //
+ x_real_est = zeros_n; // Estimation of the real states
+ // x_ns_est = zeros_n; // Estimation of the states of the no-slip system
+
+ // The estimation of measurement errors in PI observer
+ Measurement_error_est = zeros_n_ME_OB;
+
+ // The degree of no-slip
+ // ratio_ft = zeros_p;
+ ratio_ft = ones_p; // No slip
+ ratio_ft_filtered = ones_p; // No slip
+
+}
+void Observer_ftRatio::start(){
+ enable = true;
+ //
+ OB.start();
+}
+void Observer_ftRatio::pause(){
+ enable = false;
+ //
+ OB.enable = false;
+}
+void Observer_ftRatio::stop(){
+ if (!enable){
+ return;
+ }
+ enable = false;
+ // Reset
+ reset();
+
+ //
+ OB.stop();
+}
+void Observer_ftRatio::reset(){
+ //
+ // Input signals
+ yc = zeros_p;
+ // States
+ ft_est = zeros_p;
+ ft_error_est = zeros_p;
+ ft_ideal_est = zeros_p;
+ //
+ x_real_est = zeros_n; // Estimation of the real states
+ // x_ns_est = zeros_n; // Estimation of the states of the no-slip system
+
+ // The estimation of measurement errors in PI observer
+ Measurement_error_est.assign(2, 0.0);
+
+ // Reset the identifier for ratio_ft(s)
+ KFID_ratio_ft_right.reset(1.0); // 1.0 for no-slip (full friction-force)
+ KFID_ratio_ft_left.reset(1.0); // 1.0 for no-slip (full friction-force)
+
+ // The degree of no-slip
+ // ratio_ft = zeros_p;
+ ratio_ft = ones_p; // No slip
+ ratio_ft_filtered = ones_p;// No slip
+
+ // Reset the low-pass filter for ratio_ft
+ LPF_ratio_ft.reset(ratio_ft);
+
+ //
+ OB.reset();
+}
+//
+void Observer_ftRatio::init(void){
+ //
+ float _A_ftOb[] = {
+ #include "A_ftOb.txt" // Load the parameter A_ftOb
+ };
+ float _By_ftOb[] = {
+ #include "By_ftOb.txt" // Load the parameter By_ftOb
+ };
+ /*
+ float _Bv1_ftOb[] = {
+ #include "Bv1_ftOb.txt" // Load the parameter Bv1_ftOb
+ };
+ */
+ float _C_ftOb[] = {
+ #include "C_ftOb.txt" // Load the parameter C_ftOb
+ };
+ //
+ float _F1_ftOb[] = {
+ #include "F1_ftOb.txt" // Load the parameter F1_ftOb
+ };
+ float _F2_ftOb[] = {
+ #include "F2_ftOb.txt" // Load the parameter F2_ftOb
+ };
+ //
+ float _L_ftOb[] = {
+ #include "L_ftOb.txt" // Load the parameter L_ftOb
+ };
+ //
+ OB.assign_At(_A_ftOb, n_OB);
+ OB.assign_Bt(_By_ftOb, n_OB, p_OB);
+ OB.assign_Cd(_C_ftOb, q_OB, n_OB);
+ //
+ OB.assign_Lt(_L_ftOb, n_OB, 0, q_OB);
+
+ //-------------------------//
+ // assign_Matrix(Bv1_ftOb, _Bv1_ftOb, n_OB, p_OB);
+ //
+ assign_Matrix(F1_ftOb, _F1_ftOb, p, n);
+ assign_Matrix(F2_ftOb, _F2_ftOb, p, p);
+
+}
+
+//
+void Observer_ftRatio::iterateOnce(const vector<float> &yc_in, const vector<float> &x_real_in){
+ if(!enable){
+ return;
+ }
+ // Insert values
+ insertSignals(yc_in, x_real_in);
+
+ // Progress one time slot
+ OB.iterateOnce();
+
+
+ // Clean up the estimation of w_ideal_avg and w_ideal_delta
+
+
+ /*
+ // for test
+ return;
+ //
+ */
+
+
+ // Get the results
+ //--------------------------//
+ size_t idx = 0;
+ // x_real_est
+ for (size_t i = 0; i < n; ++i){
+ x_real_est[i] = OB.state_est[idx];
+ //
+ idx++;
+ }
+
+ // ft_est
+ for (size_t i = 0; i < p; ++i){
+ ft_est[i] = OB.state_est[idx];
+ //
+ idx++;
+ }
+
+ // Measurement_error_est
+ for (size_t i = 0; i < n_ME_OB; ++i){
+ Measurement_error_est[i] = OB.state_est[idx];
+ //
+ idx++;
+ }
+ //--------------------------//
+
+
+
+ // ft_ideal_est
+ // ft_ideal_est = F1_ftOb*x_real_est + F2_ftOb*yc
+ ft_ideal_est = Mat_multiply_Vec(F1_ftOb, x_real_est);
+ Get_VectorIncrement(ft_ideal_est, Mat_multiply_Vec(F2_ftOb, yc), false); // +=
+
+ // ft_error_est = ft_est - ft_ideal_est;
+ ft_error_est = Get_VectorPlus(ft_est, ft_ideal_est, true); // minus
+
+ /*
+ // The ratio_ft
+ for (size_t i = 0; i < p; ++i){
+ //
+ if (ft_ideal_est[i] < NUM_THRESHOLD && (-ft_ideal_est[i]) < NUM_THRESHOLD){
+ ratio_ft[i] = 1.0; // no-slip
+ }else{
+ ratio_ft[i] = ft_est[i]/ft_ideal_est[i];
+ }
+ // Saturation
+ if (ratio_ft[i] > 1.0){
+ ratio_ft[i] = 1.0;
+ }else if (ratio_ft[i] < 0.0){
+ ratio_ft[i] = 0.0;
+ }
+ //
+ }
+ */
+
+ // Identify the ratio_ft
+ //----------------------------------------------------//
+ /*
+ // Transform from (rifht, left) to (average, delta)
+ T_ft_est.assign_R_L(ft_est);
+ T_ft_ideal_est.assign_R_L(ft_ideal_est);
+
+ // ratio_ft_avg
+ KFID_ratio_ft_avg.C = T_ft_ideal_est.get_avg();
+ ratio_ft[0] = KFID_ratio_ft_avg.filter(0.0, T_ft_est.get_avg());
+ // ratio_ft_delta
+ KFID_ratio_ft_delta.C = T_ft_ideal_est.get_delta();
+ ratio_ft[1] = KFID_ratio_ft_delta.filter(0.0, T_ft_est.get_delta());
+ */
+
+ // Right-side ratio
+ KFID_ratio_ft_right.C = ft_ideal_est[0];
+ ratio_ft[0] = KFID_ratio_ft_right.filter(0.0, ft_est[0] );
+ // Left-side ratio
+ KFID_ratio_ft_left.C = ft_ideal_est[1];
+ ratio_ft[1] = KFID_ratio_ft_left.filter(0.0, ft_est[1] );
+
+ // Saturation
+ ratio_ft = SAT_ratio_ft.filter(ratio_ft);
+ //----------------------------------------------------//
+
+ // Filtering
+ ratio_ft_filtered = LPF_ratio_ft.filter(ratio_ft);
+
+}
+
+// Private methods
+//
+void Observer_ftRatio::insertSignals(const vector<float> &yc_in, const vector<float> &x_real_in){
+ //
+ yc = yc_in;
+ // Insert values
+ OB.sys_inputs = yc_in;
+ OB.sys_outputs = x_real_in;
+ //
+ // OB.sys_extraDisturbance = Mat_multiply_Vec(Bv1_ftOb, v1_in);
+}
+
+
+
+// Utilities
+void Observer_ftRatio::Mat_multiply_Vec(vector<float> &v_out, const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
+
+ // Size check
+ if (v_out.size() != m_left.size()){
+ v_out.resize(m_left.size());
+ }
+
+ /*
+ // Iterators
+ static vector<float>::iterator it_out;
+ static vector<float>::iterator it_m_row;
+ static vector<float>::iterator it_v;
+
+ //
+ it_out = v_out.begin();
+ for (size_t i = 0; i < m_left.size(); ++i){
+ *it_out = 0.0;
+ it_m_row = vector<vector<float> >(m_left)[i].begin();
+ it_v = vector<float>(v_right).begin();
+ for (size_t j = 0; j < m_left[i].size(); ++j){
+ // *it_out += m_left[i][j] * v_right[j];
+ if (*it_m_row != 0.0 && *it_v != 0.0){
+ (*it_out) += (*it_m_row) * (*it_v);
+ }else{
+ // (*it_out) += 0.0
+ }
+ // (*it_out) += (*it_m_row) * (*it_v);
+ //
+ it_m_row++;
+ it_v++;
+ }
+ it_out++;
+ }
+ */
+
+ // Indexing
+ for (size_t i = 0; i < m_left.size(); ++i){
+ //
+ v_out[i] = 0.0;
+ //
+ for (size_t j = 0; j < v_right.size(); ++j){
+ if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
+ v_out[i] += m_left[i][j]*v_right[j];
+ }
+ }
+
+}
+vector<float> Observer_ftRatio::Mat_multiply_Vec(const vector<vector<float> > &m_left, const vector<float> &v_right){ // v_out = m_left*v_right
+ static vector<float> v_out;
+ // Size check
+ if (v_out.size() != m_left.size()){
+ v_out.resize(m_left.size());
+ }
+
+ /*
+ // Iterators
+ static vector<float>::iterator it_out;
+ static vector<float>::iterator it_m_row;
+ static vector<float>::iterator it_v;
+ //
+ it_out = v_out.begin();
+ for (size_t i = 0; i < m_left.size(); ++i){
+ *it_out = 0.0;
+ it_m_row = vector<vector<float> >(m_left)[i].begin();
+ it_v = vector<float>(v_right).begin();
+ for (size_t j = 0; j < m_left[i].size(); ++j){
+ // *it_out += m_left[i][j] * v_right[j];
+ if (*it_m_row != 0.0 && *it_v != 0.0){
+ (*it_out) += (*it_m_row) * (*it_v);
+ }else{
+ // (*it_out) += 0.0
+ }
+ // (*it_out) += (*it_m_row) * (*it_v);
+ //
+ it_m_row++;
+ it_v++;
+ }
+ it_out++;
+ }
+ */
+
+ // Indexing
+ for (size_t i = 0; i < m_left.size(); ++i){
+ //
+ v_out[i] = 0.0;
+ //
+ for (size_t j = 0; j < v_right.size(); ++j){
+ if (m_left[i][j] != 0.0 && v_right[j] != 0.0)
+ v_out[i] += m_left[i][j]*v_right[j];
+ }
+ }
+
+ return v_out;
+}
+vector<float> Observer_ftRatio::Get_VectorPlus(const vector<float> &v_a, const vector<float> &v_b, bool is_minus) // v_a + (or -) v_b
+{
+ static vector<float> v_c;
+ // Size check
+ if (v_c.size() != v_a.size()){
+ v_c.resize(v_a.size());
+ }
+ //
+ for (size_t i = 0; i < v_a.size(); ++i){
+ if (is_minus){
+ v_c[i] = v_a[i] - v_b[i];
+ }else{
+ v_c[i] = v_a[i] + v_b[i];
+ }
+ }
+ return v_c;
+}
+vector<float> Observer_ftRatio::Get_VectorScalarMultiply(const vector<float> &v_a, float scale) // scale*v_a
+{
+ static vector<float> v_c;
+ // Size check
+ if (v_c.size() != v_a.size()){
+ v_c.resize(v_a.size());
+ }
+ // for pure negative
+ if (scale == -1.0){
+ for (size_t i = 0; i < v_a.size(); ++i){
+ v_c[i] = -v_a[i];
+ }
+ return v_c;
+ }
+ // else
+ for (size_t i = 0; i < v_a.size(); ++i){
+ v_c[i] = scale*v_a[i];
+
+ }
+ return v_c;
+}
+// Increment
+void Observer_ftRatio::Get_VectorIncrement(vector<float> &v_a, const vector<float> &v_b, bool is_minus){ // v_a += (or -=) v_b
+ // Size check
+ if (v_a.size() != v_b.size()){
+ v_a.resize(v_b.size());
+ }
+ //
+ if (is_minus){ // -=
+ for (size_t i = 0; i < v_b.size(); ++i){
+ v_a[i] -= v_b[i];
+ }
+ }else{ // +=
+ for (size_t i = 0; i < v_b.size(); ++i){
+ v_a[i] += v_b[i];
+ }
+ }
+
+}
+
+//
+void Observer_ftRatio::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
+ // M_in is the pointer of a mutidimentional array with size m_in by n_in
+ M.resize(m_in, vector<float>(n_in, 0.0));
+ //
+ for (size_t i = 0; i < m_in; ++i){
+ for (size_t j = 0; j < n_in; ++j){
+ // M[i][j] = M_in[i][j];
+ M[i][j] = *M_in;
+ M_in++;
+ }
+ }
+}