1 | 1 |
|
2 | 2 |
import org.apache.commons.math3.distribution.MultivariateNormalDistribution;
|
3 | 3 |
|
|
4 |
import java.io.BufferedWriter;
|
|
5 |
import java.io.FileWriter;
|
|
6 |
import java.io.IOException;
|
|
7 |
import java.io.OutputStreamWriter;
|
|
8 |
|
|
9 |
import java.text.SimpleDateFormat;
|
4 | 10 |
import java.util.ArrayList;
|
|
11 |
import java.util.Date;
|
5 | 12 |
import java.util.PriorityQueue;
|
6 | 13 |
import java.util.Random;
|
7 | |
import java.util.TreeSet;
|
|
14 |
import java.util.HashSet;
|
8 | 15 |
|
9 | 16 |
public class PidTuner implements Updatable {
|
10 | 17 |
private static enum TuningAxis { DX, DY, DZ, DT };
|
|
23 | 30 |
// Number of children to expand from each selected node.
|
24 | 31 |
private static final int EXPAND_NUM = 3;
|
25 | 32 |
|
|
33 |
private static SimpleDateFormat dateFormat =
|
|
34 |
new SimpleDateFormat("EEE, d MMM yyyy HH:mm:ss");
|
|
35 |
|
26 | 36 |
private ArrayList<PidExperiment> mFringe;
|
27 | |
private TreeSet<PidExperiment> mHistory;
|
|
37 |
private HashSet<PidExperiment> mHistory;
|
28 | 38 |
private int mFringeIndex;
|
29 | 39 |
// Covariance matrix for expanding nodes.
|
30 | 40 |
private double[][] mExpCovar;
|
31 | 41 |
private Random rn;
|
|
42 |
private BufferedWriter output;
|
32 | 43 |
|
33 | 44 |
private boolean mEnabled = false;
|
34 | 45 |
|
|
40 | 51 |
mAxis = TuningAxis.DX;
|
41 | 52 |
} else if (axis.equals("dy")) {
|
42 | 53 |
mAxis = TuningAxis.DY;
|
|
54 |
} else if (axis.equals("dt")) {
|
|
55 |
mAxis = TuningAxis.DT;
|
43 | 56 |
} else {
|
44 | 57 |
mEnabled = false;
|
45 | 58 |
}
|
|
48 | 61 |
return;
|
49 | 62 |
}
|
50 | 63 |
mFringe = new ArrayList<PidExperiment>();
|
51 | |
mHistory = new TreeSet<PidExperiment>();
|
|
64 |
mHistory = new HashSet<PidExperiment>();
|
52 | 65 |
mFringeIndex = 0;
|
53 | 66 |
rn = new Random();
|
54 | |
|
|
67 |
try {
|
|
68 |
output = new BufferedWriter(new FileWriter("tuning_" + mAxis + ".txt", true));
|
|
69 |
output.write("# "+ dateFormat.format(new Date()));
|
|
70 |
output.newLine();
|
|
71 |
output.write("# tuning " + mAxis + " select_num " + SELECT_NUM + " expand_num " +
|
|
72 |
EXPAND_NUM);
|
|
73 |
output.newLine();
|
|
74 |
} catch (IOException e) {
|
|
75 |
Debug.log("WARNING: PID TUNING LOGGING FAILED.");
|
|
76 |
e.printStackTrace();
|
|
77 |
}
|
55 | 78 |
mExpCovar = new double[3][3];
|
56 | 79 |
for (int i = 0; i < 3; i++) {
|
57 | 80 |
for (int j = 0; j < 3; j++) {
|
|
84 | 107 |
currentExp.addError(error);
|
85 | 108 |
// If PidE not done, return;
|
86 | 109 |
if (!currentExp.isDone()) return;
|
87 | |
// PidE done: increment index
|
|
110 |
// PidE done: record result, increment index
|
|
111 |
try {
|
|
112 |
if (output != null) {
|
|
113 |
output.write(currentExp.gnuplotLine());
|
|
114 |
output.newLine();
|
|
115 |
output.flush();
|
|
116 |
}
|
|
117 |
} catch (IOException e) {
|
|
118 |
Debug.log("WARNING: PID TUNING LOGGING FAILED");
|
|
119 |
e.printStackTrace();
|
|
120 |
}
|
88 | 121 |
mFringeIndex++;
|
89 | 122 |
// If index <= mFringe.size(), send new PID values, return;
|
90 | 123 |
if (mFringeIndex <= mFringe.size()) {
|
|
138 | 171 |
mFringe.add(child);
|
139 | 172 |
}
|
140 | 173 |
}
|
141 | |
// For debugging:
|
142 | |
System.out.println("Best PID error so far: " + mHistory.first().getScore());
|
143 | |
System.out.println(mHistory.first().getP() + ", " + mHistory.first().getI() + ", " + mHistory.first().getD());
|
144 | 174 |
}
|
145 | 175 |
}
|