/// <summary>
/// Extremum Seeking-based synergistic human-prosthetic interface personalisation algorithm. Based on:
/// Garcia-Rosas, Riccardo, et al. "On-line synergy identification for personalized active arm prosthesis: a feasibility study."
/// 2018 Annual American Control Conference (ACC). IEEE, 2018.
/// </summary>
/// <param name="a">The dither signal amplitude.</param>
/// <param name="wd">The dither signal frequency (rad/s).</param>
/// <param name="phi">The dither signal phase shift (rad).</param>
/// <param name="wo"> The estimator frequency, well bellow wd, see Garcia-Rosas ACC 2018.</param>
/// <param name="L">The gradient estimator gain (one dimensional in this case).</param>
/// <param name="ts">The system sampling time (sec).</param>
/// <param name="k"> The optimiser gain. </param>
/// <param name="theta_0"> The HPI parameter initial condition. </param>
/// <param name="thetaMin"> The lower bound of the HPI parameter. </param>
/// <param name="thetaMax"> The upper bound of the HPI parameter. </param>
public ACCHPIPersonalisation(float a, float wd, float phi, float wo, float[] L, float ts, float k, float theta_0, float thetaMin, float thetaMax)
{
estimator = new GradientDemodulator(a, wd, phi, wo, L, ts);
optimiser = new GradientOptimiser(k, ts, theta_0);
dither = new SinusoidalDither(a, wd, phi);
SetParameterLimits(thetaMin, thetaMax);
}
/// <summary>
/// Fast Extremum Seeking-based synergistic human-prosthetic interface personalisation algorithm. Based on:
/// Garcia-Rosas R., Tan Y., Oetomo D., Manzie C., Choong P. "Personalized On-line Adaptation of
/// Kinematic Synergies for Human-Prosthesis Interfaces". Transactions on Cybernetics. 2019.
/// </summary>
/// <param name="a"> The dither signal amplitude. </param>
/// <param name="phi"> The dither signal phase shift (rad) .</param>
/// <param name="wo"> The excitation and estimation dither vector frequencies. [0]: excitation dither and gradient estimation,
/// [1]: hessian estimation. From paper: [0]: wd, [1]: 2*wd. </param>
/// <param name="L"> The observer gain. </param>
/// <param name="ts"> The system sampling time (sec). </param>
/// <param name="k"> The optimiser gain. </param>
/// <param name="epsilon"> The optimiser switching threshold. </param>
/// <param name="A"> The filter A matrix (state-space representation). </param>
/// <param name="B"> The filter B matrix (state-space representation). </param>
/// <param name="C"> The filter C matrix (state-space representation). </param>
/// <param name="D"> The filter D matrix (state-space representation). </param>
/// <param name="theta_0"> The HPI parameter initial condition. </param>
/// <param name="thetaMin"> The lower bound of the HPI parameter. </param>
/// <param name="thetaMax"> The upper bound of the HPI parameter. </param>
public TransCyberHPIPersonalisation(float a, float phi, float[] wo, float[] L, float ts, float k, float epsilon, float[][] A, float[] B, float[] C, float D, float theta_0, float thetaMin, float thetaMax)
{
filter = new NDegreeSSDFilter(A, B, C, D);
estimator = new GradientHessianObserver(wo, L, GradientHessianObserver.ObserverType.GradientHessian, a);
optimiser = new SGNOptimiser(k, wo[0], epsilon, theta_0);
dither = new SinusoidalDither(a, wo[0], phi);
SetParameterLimits(thetaMin, thetaMax);
}
/// <summary>
/// Creates a basic first order gradient estimator using a demodulation method with first order high and low pass filters.
/// Uses a sinusoidal dither signal for demodulation.
/// Creates the filters with cutt-off frequency of wd/10.
/// </summary>
/// <param name="wd">The dither frequency (rad/s), must satisfy certain conditions, see Garcia-Rosas ACC 2018.</param>
/// <param name="L"></param>
/// <param name="ts"></param>
public GradientDemodulator(float wd, float[] L, float ts)
{
float[] woArr = { wd };
SetEstimatorFrequencies(woArr);
SetGains(L);
SetSamplingTime(ts);
dither = new SinusoidalDither(1.0f, wd, 0.0f);
hpf = new FOHPDFilter(wd / 10.0f, L[0], ts);
lpf = new FOLPDFilter(wd / 10.0f, L[0], ts);
}
/// <summary>
/// Creates a basic first order gradient estimator using a demodulation method with first order high and low pass filters.
/// Uses a sinusoidal dither signal for demodulation.
/// </summary>
/// <param name="a">The dither signal amplitude.</param>
/// <param name="wd">The dither frequency (rad/s), must satisfy certain conditions, see Garcia-Rosas ACC 2018.</param>
/// <param name="phi">The dither signal phase shift.</param>
/// <param name="wo">Estimator frequency, well bellow wd, see Garcia-Rosas ACC 2018.</param>
/// <param name="L">Estimator gains, one dimensional for this type.</param>
/// <param name="ts">The estimator (system) sampling time.</param>
public GradientDemodulator(float a, float wd, float phi, float wo, float[] L, float ts)
{
float[] woArr = { wo };
SetEstimatorFrequencies(woArr);
SetGains(L);
SetSamplingTime(ts);
dither = new SinusoidalDither(a, wd, phi);
hpf = new FOHPDFilter(wo, L[0], ts);
lpf = new FOLPDFilter(wo, L[0], ts);
}
