├── LICENSE ├── README.md ├── adrc.c └── adrc.h /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Jiachi Zou 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Active Disturbance Control (ADRC) Library 2 | ======================================= 3 | -------------------------------------------------------------------------------- /adrc.c: -------------------------------------------------------------------------------- 1 | /* 2 | * File : adrc.c 3 | * 4 | * 5 | * Change Logs: 6 | * Date Author Notes 7 | * 2018-03-18 zoujiachi the first version 8 | */ 9 | 10 | #include 11 | #include 12 | 13 | static float adrc_sign(float val) 14 | { 15 | if(val >= 0.0f) 16 | return 1.0f; 17 | else 18 | return -1.0f; 19 | } 20 | 21 | float adrc_fhan(float v1, float v2, float r0, float h0) 22 | { 23 | float d = h0 * h0 * r0; 24 | float a0 = h0 * v2; 25 | float y = v1 + a0; 26 | float a1 = sqrtf(d*(d + 8.0f*fabsf(y))); 27 | float a2 = a0 + adrc_sign(y)*(a1-d)*0.5f; 28 | float sy = (adrc_sign(y+d) - adrc_sign(y-d))*0.5f; 29 | float a = (a0 + y - a2)*sy + a2; 30 | float sa = (adrc_sign(a+d) - adrc_sign(a-d))*0.5f; 31 | 32 | return -r0*(a/d - adrc_sign(a))*sa - r0*adrc_sign(a); 33 | } 34 | 35 | float adrc_fal(float e, float alpha, float delta) 36 | { 37 | if(fabsf(e) <= delta){ 38 | return e / (powf(delta, 1.0f-alpha)); 39 | }else{ 40 | return powf(fabsf(e), alpha) * adrc_sign(e); 41 | } 42 | } 43 | 44 | void adrc_td_init(ADRC_TD_Def* td_t, float h, float r0, float h0) 45 | { 46 | td_t->h = h; 47 | td_t->r0 = r0; 48 | td_t->h0 = h0; 49 | td_t->v1 = td_t->v2 = 0.0f; 50 | } 51 | 52 | void adrc_td(ADRC_TD_Def* td, float v) 53 | { 54 | float fv = adrc_fhan(td->v1 - v, td->v2, td->r0, td->h0); 55 | 56 | td->v1 += td->h * td->v2; 57 | td->v2 += td->h * fv; 58 | } 59 | 60 | void adrc_td_control_init(TD_Controller_Def* td_controller, float h, float r2, float h2) 61 | { 62 | td_controller->v1 = 0.0f; 63 | td_controller->v2 = 0.0f; 64 | td_controller->r2 = r2; 65 | td_controller->h2 = h2; 66 | td_controller->h = h; 67 | } 68 | 69 | float adrc_td_control(TD_Controller_Def* td_controller, float err) 70 | { 71 | float fv = adrc_fhan(-err, td_controller->v2, td_controller->r2, td_controller->h2); 72 | td_controller->v1 += td_controller->h * td_controller->v2; 73 | td_controller->v2 += td_controller->h * fv; 74 | 75 | return td_controller->v2; 76 | } 77 | 78 | void adrc_eso_init(ADRC_ESO_Def* eso_t, float h, float beta1, float beta2, float alpha, float delta, float b0) 79 | { 80 | eso_t->h = h; 81 | eso_t->beta1 = beta1; 82 | eso_t->beta2 = beta2; 83 | eso_t->u = 0.0f; 84 | eso_t->alpha = alpha; 85 | eso_t->delta = delta; 86 | eso_t->b0 = b0; 87 | 88 | eso_t->z1 = eso_t->z2 = 0.0f; 89 | } 90 | 91 | void adrc_eso(ADRC_ESO_Def* eso_t, float y) 92 | { 93 | float e = eso_t->z1 - y; 94 | float fe = adrc_fal(e, eso_t->alpha, eso_t->delta); 95 | 96 | eso_t->z1 += eso_t->h*(eso_t->z2 + eso_t->b0*eso_t->u - eso_t->beta1*e); 97 | eso_t->z2 -= eso_t->h*eso_t->beta2*fe; 98 | } 99 | 100 | void adrc_leso_init(ADRC_LESO_Def* leso_t, float h, float w, float b0) 101 | { 102 | leso_t->h = h; 103 | // (s + w)^2 = s^2 + beta_1 * s + beta_2 104 | leso_t->beta1 = 2.0f*w; 105 | leso_t->beta2 = w*w; 106 | leso_t->u = 0.0f; 107 | leso_t->b0 = b0; 108 | 109 | leso_t->z1 = leso_t->z2 = 0.0f; 110 | } 111 | 112 | void adrc_leso(ADRC_LESO_Def* leso_t, float y) 113 | { 114 | float e = leso_t->z1 - y; 115 | 116 | leso_t->z1 += leso_t->h*(leso_t->z2 + leso_t->b0*leso_t->u - leso_t->beta1*e); 117 | leso_t->z2 -= leso_t->h*leso_t->beta2*e; 118 | } 119 | 120 | void adrc_nlsef_init(ADRC_NLSEF_Def* nlsef_t, float h, float r1, float h1, float c) 121 | { 122 | nlsef_t->h = h; 123 | nlsef_t->h1 = h1; 124 | nlsef_t->r1 = r1; 125 | nlsef_t->c = c; 126 | } 127 | 128 | float adrc_nlsef(ADRC_NLSEF_Def* nlsef_t, float e1, float e2) 129 | { 130 | return -adrc_fhan(e1, nlsef_t->c*e2, nlsef_t->r1, nlsef_t->h1); 131 | } -------------------------------------------------------------------------------- /adrc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File : adrc.h 3 | * 4 | * 5 | * Change Logs: 6 | * Date Author Notes 7 | * 2018-03-18 zoujiachi the first version 8 | */ 9 | 10 | #ifndef __ADRC_H__ 11 | #define __ADRC_H__ 12 | 13 | typedef struct 14 | { 15 | float v1; 16 | float v2; 17 | float r2; 18 | float h2; 19 | float h; 20 | }TD_Controller_Def; 21 | 22 | typedef struct 23 | { 24 | float v1; 25 | float v2; 26 | float r0; 27 | float h0; 28 | float h; 29 | }ADRC_TD_Def; 30 | 31 | typedef struct 32 | { 33 | float h; 34 | float beta1; 35 | float beta2; 36 | float alpha; 37 | float delta; 38 | float u; 39 | float b0; 40 | /* ESO */ 41 | float z1; 42 | float z2; 43 | }ADRC_ESO_Def; 44 | 45 | typedef struct 46 | { 47 | float h; 48 | float beta1; 49 | float beta2; 50 | float u; 51 | float b0; 52 | /* LESO */ 53 | float z1; 54 | float z2; 55 | }ADRC_LESO_Def; /* Linear ESO */ 56 | 57 | typedef struct 58 | { 59 | float h; 60 | float h1; 61 | float r1; 62 | float c; 63 | }ADRC_NLSEF_Def; 64 | 65 | void adrc_td_init(ADRC_TD_Def* td_t, float h, float r0, float h0); 66 | void adrc_td(ADRC_TD_Def* td, float v); 67 | void adrc_td_control_init(TD_Controller_Def* td_controller, float h, float r2, float h2); 68 | float adrc_td_control(TD_Controller_Def* td_controller, float err); 69 | void adrc_eso_init(ADRC_ESO_Def* eso_t, float h, float beta1, float beta2, float alpha, float delta, float b0); 70 | void adrc_eso(ADRC_ESO_Def* eso_t, float y); 71 | void adrc_leso_init(ADRC_LESO_Def* leso_t, float h, float w, float b0); 72 | void adrc_leso(ADRC_LESO_Def* leso_t, float y); 73 | void adrc_nlsef_init(ADRC_NLSEF_Def* nlsef_t, float h, float r1, float h1, float c); 74 | float adrc_nlsef(ADRC_NLSEF_Def* nlsef_t, float e1, float e2); 75 | 76 | #endif 77 | 78 | --------------------------------------------------------------------------------