You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
47 lines
1.5 KiB
47 lines
1.5 KiB
13 years ago
|
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);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|