mirror of https://github.com/ArduPilot/ardupilot
47 lines
1.5 KiB
C#
47 lines
1.5 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
|
|
namespace ArdupilotMega
|
|
{
|
|
class PIDTunning
|
|
{
|
|
|
|
public static void twiddle(double[] initialgains, Func<double[],double> run, double tol = 0.001)
|
|
{
|
|
int n_params = 3;
|
|
double err= 0;
|
|
double[] dparams = initialgains; //{1.0f,1.0f,1.0f};
|
|
double[] paramss = {0.0f,0.0f,0.0f};
|
|
double best_error = run(paramss);
|
|
int n = 0;
|
|
|
|
while (dparams.Sum() > tol) {
|
|
for (int i = 0; i < n_params; i++){
|
|
paramss[i] += dparams[i];
|
|
err = run(paramss);
|
|
if (err < best_error){
|
|
best_error = err;
|
|
dparams[i] *= 1.1;
|
|
}
|
|
else {
|
|
paramss[i] -= 2.0 * dparams[i];
|
|
err = run(paramss);
|
|
if (err < best_error){
|
|
best_error = err;
|
|
dparams[i] *= 1.1;
|
|
}
|
|
else {
|
|
paramss[i] += dparams[i];
|
|
dparams[i] *= 0.9;
|
|
}
|
|
}
|
|
n += 1;
|
|
Console.WriteLine("Twiddle #" + n + " " + paramss.ToString() + " -> " + best_error);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|