src/pid_fuzzy.c

   1 #include "a/pid_fuzzy.h"
   2 #include "a/mf.h"
   3
   4 a_float (*a_pid_fuzzy_op(unsigned int op))(a_float, a_float)
   5 {
   6     switch (op)
   7     {
   8     default:
   9     case A_PID_FUZZY_EQU:
  10         return a_fuzzy_equ;
  11     case A_PID_FUZZY_CAP:
  12         return a_fuzzy_cap;
  13     case A_PID_FUZZY_CAP_ALGEBRA:
  14         return a_fuzzy_cap_algebra;
  15     case A_PID_FUZZY_CAP_BOUNDED:
  16         return a_fuzzy_cap_bounded;
  17     case A_PID_FUZZY_CUP:
  18         return a_fuzzy_cup;
  19     case A_PID_FUZZY_CUP_ALGEBRA:
  20         return a_fuzzy_cup_algebra;
  21     case A_PID_FUZZY_CUP_BOUNDED:
  22         return a_fuzzy_cup_bounded;
  23     }
  24 }
  25
  26 void a_pid_fuzzy_set_op(a_pid_fuzzy *ctx, unsigned int op) { ctx->op = a_pid_fuzzy_op(op); }
  27
  28 A_HIDDEN unsigned int a_pid_fuzzy_mf(a_float x, unsigned int n, a_float const *a, unsigned int *idx, a_float *val);
  29 unsigned int a_pid_fuzzy_mf(a_float x, unsigned int n, a_float const *a, unsigned int *idx, a_float *val)
  30 {
  31     unsigned int counter = 0;
  32     for (unsigned int i = 0; i != n; ++i)
  33     {
  34         a_float y = 0;
  35         switch ((int)*a++)
  36         {
  37         default:
  38         case A_MF_NUL: goto out;
  39         case A_MF_GAUSS:
  40             y = a_mf_gauss(x, a[0], a[1]);
  41             a += 2;
  42             break;
  43         case A_MF_GAUSS2:
  44             y = a_mf_gauss2(x, a[0], a[1], a[2], a[3]);
  45             a += 4;
  46             break;
  47         case A_MF_GBELL:
  48             y = a_mf_gbell(x, a[0], a[1], a[2]);
  49             a += 3;
  50             break;
  51         case A_MF_SIG:
  52             y = a_mf_sig(x, a[0], a[1]);
  53             a += 2;
  54             break;
  55         case A_MF_DSIG:
  56             y = a_mf_dsig(x, a[0], a[1], a[2], a[3]);
  57             a += 4;
  58             break;
  59         case A_MF_PSIG:
  60             y = a_mf_psig(x, a[0], a[1], a[2], a[3]);
  61             a += 4;
  62             break;
  63         case A_MF_TRAP:
  64             y = a_mf_trap(x, a[0], a[1], a[2], a[3]);
  65             a += 4;
  66             break;
  67         case A_MF_TRI:
  68             y = a_mf_tri(x, a[0], a[1], a[2]);
  69             a += 3;
  70             break;
  71         case A_MF_LINS:
  72             y = a_mf_lins(x, a[0], a[1]);
  73             a += 2;
  74             break;
  75         case A_MF_LINZ:
  76             y = a_mf_linz(x, a[0], a[1]);
  77             a += 2;
  78             break;
  79         case A_MF_S:
  80             y = a_mf_s(x, a[0], a[1]);
  81             a += 2;
  82             break;
  83         case A_MF_Z:
  84             y = a_mf_z(x, a[0], a[1]);
  85             a += 2;
  86             break;
  87         case A_MF_PI:
  88             y = a_mf_pi(x, a[0], a[1], a[2], a[3]);
  89             a += 4;
  90             break;
  91         }
  92         if (y > A_FLOAT_EPSILON)
  93         {
  94             *idx++ = i;
  95             *val++ = y;
  96             ++counter;
  97         }
  98     }
  99 out:
 100     return counter;
 101 }
 102
 103 void a_pid_fuzzy_rule(a_pid_fuzzy *ctx, unsigned int order, a_float const *me, a_float const *mec,
 104                       a_float const *mkp, a_float const *mki, a_float const *mkd)
 105 {
 106     ctx->me = me;
 107     ctx->mec = mec;
 108     ctx->mkp = mkp;
 109     ctx->mki = mki;
 110     ctx->mkd = mkd;
 111     ctx->order = order;
 112 }
 113
 114 void *a_pid_fuzzy_block(a_pid_fuzzy *ctx) { return ctx->idx; }
 115 void a_pid_fuzzy_set_block(a_pid_fuzzy *ctx, void *ptr, a_size num)
 116 {
 117     ctx->block = (unsigned int)num;
 118     ctx->idx = (unsigned int *)ptr;
 119     ptr = (a_byte *)ptr + 2 * sizeof(unsigned int) * num;
 120     ctx->val = (a_float *)ptr;
 121 }
 122
 123 void a_pid_fuzzy_kpid(a_pid_fuzzy *ctx, a_float kp, a_float ki, a_float kd)
 124 {
 125     a_pid_kpid(&ctx->pid, kp, ki, kd);
 126     ctx->kp = kp;
 127     ctx->ki = ki;
 128     ctx->kd = kd;
 129 }
 130
 131 A_HIDDEN void a_pid_fuzzy_out_(a_pid_fuzzy *ctx, a_float ec, a_float e);
 132 void a_pid_fuzzy_out_(a_pid_fuzzy *ctx, a_float ec, a_float e)
 133 {
 134     a_float kp = 0;
 135     a_float ki = 0;
 136     a_float kd = 0;
 137     /* calculate membership */
 138     unsigned int const ne = a_pid_fuzzy_mf(e, ctx->order, ctx->me, ctx->idx, ctx->val);
 139     if (!ne) { goto pid; }
 140     unsigned int *const idx = ctx->idx + ne;
 141     a_float *const val = ctx->val + ne;
 142     unsigned int const nec = a_pid_fuzzy_mf(ec, ctx->order, ctx->mec, idx, val);
 143     if (!nec) { goto pid; }
 144     a_float *const mat = val + nec;
 145     /* joint membership */
 146     a_float inv = 0;
 147     {
 148         a_float *it = mat;
 149         for (unsigned int i = 0; i != ne; ++i)
 150         {
 151             for (unsigned int j = 0; j != nec; ++j)
 152             {
 153                 *it = ctx->op(ctx->val[i], val[j]); /* mat(i,j)=f(e[i],ec[j]) */
 154                 inv += *it++;
 155             }
 156             ctx->idx[i] *= ctx->order;
 157         }
 158     }
 159     inv = 1 / inv;
 160     /* mean of centers defuzzifier */
 161     if (ctx->mkp)
 162     {
 163         a_float const *it = mat;
 164         for (unsigned int i = 0; i != ne; ++i)
 165         {
 166             a_float const *const mkp = ctx->mkp + ctx->idx[i];
 167             for (unsigned int j = 0; j != nec; ++j)
 168             {
 169                 kp += *it++ * mkp[idx[j]]; /* += mat(i,j) * mkp(e[i],ec[j]) */
 170             }
 171         }
 172         kp *= inv;
 173     }
 174     if (ctx->mki)
 175     {
 176         a_float const *it = mat;
 177         for (unsigned int i = 0; i != ne; ++i)
 178         {
 179             a_float const *const mki = ctx->mki + ctx->idx[i];
 180             for (unsigned int j = 0; j != nec; ++j)
 181             {
 182                 ki += *it++ * mki[idx[j]]; /* += mat(i,j) * mki(e[i],ec[j]) */
 183             }
 184         }
 185         ki *= inv;
 186     }
 187     if (ctx->mkd)
 188     {
 189         a_float const *it = mat;
 190         for (unsigned int i = 0; i != ne; ++i)
 191         {
 192             a_float const *const mkd = ctx->mkd + ctx->idx[i];
 193             for (unsigned int j = 0; j != nec; ++j)
 194             {
 195                 kd += *it++ * mkd[idx[j]]; /* += mat(i,j) * mkd(e[i],ec[j]) */
 196             }
 197         }
 198         kd *= inv;
 199     }
 200 pid:
 201     a_pid_kpid(&ctx->pid, ctx->kp + kp, ctx->ki + ki, ctx->kd + kd);
 202 }
 203
 204 A_HIDDEN a_float a_pid_run_(a_pid *ctx, a_float set, a_float fdb, a_float err);
 205 a_float a_pid_fuzzy_run(a_pid_fuzzy *ctx, a_float set, a_float fdb)
 206 {
 207     a_float const err = set - fdb;
 208     a_pid_fuzzy_out_(ctx, err - ctx->pid.err, err);
 209     return a_pid_run_(&ctx->pid, set, fdb, err);
 210 }
 211
 212 A_HIDDEN a_float a_pid_pos_(a_pid *ctx, a_float fdb, a_float err);
 213 a_float a_pid_fuzzy_pos(a_pid_fuzzy *ctx, a_float set, a_float fdb)
 214 {
 215     a_float const err = set - fdb;
 216     a_pid_fuzzy_out_(ctx, err - ctx->pid.err, err);
 217     return a_pid_pos_(&ctx->pid, fdb, err);
 218 }
 219
 220 A_HIDDEN a_float a_pid_inc_(a_pid *ctx, a_float fdb, a_float err);
 221 a_float a_pid_fuzzy_inc(a_pid_fuzzy *ctx, a_float set, a_float fdb)
 222 {
 223     a_float const err = set - fdb;
 224     a_pid_fuzzy_out_(ctx, err - ctx->pid.err, err);
 225     return a_pid_inc_(&ctx->pid, fdb, err);
 226 }
 227
 228 void a_pid_fuzzy_zero(a_pid_fuzzy *ctx) { a_pid_zero(&ctx->pid); }