-
Notifications
You must be signed in to change notification settings - Fork 9
/
feedforwardAutoTuning.m
74 lines (57 loc) · 2.37 KB
/
feedforwardAutoTuning.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
%% code section A: tuning of acc ff
feedbackControlSignal = ufb.signals.values;
accSignal = acc.signals.values;
simTime = ufb.time;
startTime = 0.008;
endTime = 0.042;
beginIndex = find(simTime > startTime);
endIndex = find(simTime > endTime);
A = accSignal(beginIndex:endIndex);
b = feedbackControlSignal(beginIndex:endIndex);
accCoefEstimated = inv(A'*A) * A' * b;
msgbox(sprintf('The tuned acceleration feedforward coefficient is %.4f kg',accCoefEstimated));
%% code section B: tuning of jerk ff
feedbackControlSignal = ufb.signals.values;
jerkSignal = jerk.signals.values;
simTime = ufb.time;
startTime = 0.0;
endTime = 0.0472;
beginIndex = find(simTime > startTime);
endIndex = find(simTime > endTime);
A = jerkSignal(beginIndex:endIndex);
b = feedbackControlSignal(beginIndex:endIndex);
jerkCoefEstimated = inv(A'*A) * A' * b;
msgbox(sprintf('The tuned jerk feedforward coefficient is %.4f kg.s',jerkCoefEstimated));
%% code section C: joint tuning of acc and jerk ff
feedbackControlSignal = ufb.signals.values;
accSignal = acc.signals.values;
jerkSignal = jerk.signals.values;
simTime = ufb.time;
startTime = 0.0;
endTime = 0.0472;
beginIndex = find(simTime > startTime);
endIndex = find(simTime > endTime);
dataIndex = beginIndex:endIndex;
A = [accSignal(dataIndex),jerkSignal(dataIndex)];
b = feedbackControlSignal(dataIndex);
deltaEstimated = inv(A'*A) * A' * b;
accCoefNew = accCoefEstimated + deltaEstimated(1);
jerkCoefNew = jerkCoefEstimated + deltaEstimated(2);
msgbox(sprintf('The tuned acc feedforward coefficient is %.4f kg, jerk feedforward coefficient is %.4f kg.s',accCoefNew, jerkCoefNew));
%% code section D: joint tuning of acc, jerk, snap ff
accSignal = acc.signals.values;
jerkSignal = jerk.signals.values;
snapSignal = snap.signals.values;
trajSignal = [accSignal,jerkSignal,snapSignal];
startTime = 0.0;
endTime = 0.0472;
beginIndex = find(simTime > startTime);
endIndex = find(simTime > endTime);
A = trajSignal(beginIndex:endIndex,:);
% b = feedbackControlSignal(beginIndex:endIndex);
b = ufbF(beginIndex:endIndex,:);
coefEstimated = inv(A'*A) * A' * b;
accCoefLast = accCoefNew + coefEstimated(1);
jerkCoefLast = jerkCoefNew + coefEstimated(2);
snapCoefLast = coefEstimated(3);
msgbox(sprintf('The tuned acc feedforward coefficient is %.4f kg, jerk feedforward coefficient is %.4f kg.s, snap feedforward coefficient is %.4e kg.s^2',accCoefLast, jerkCoefLast,snapCoefLast));