Here is the data if anyone wants to analyze it: https://dl.dropbox.com/u/6645063/log_20130204_064517.mat . There is a new object "SystemIdent" which is the superimposed signal for each channel (pre output scaling so it has an arbitrary scale).
You can see it really denoises the gyro trace a lot without introducing any latency. I'm definitely tempted to code it up and try using that signal in the PID controller instead of the raw gyro. If nothing else it will greatly reduce the noise on the output which will reduce power consumption a lot. If that works possibly it can be used for a nicer controller than PID.
% so we have state at each step in time
% want to solve for control output that gets to
% [0 0 0] 20 steps out
Q = diag([10 10 1e-5 0]);
R = 1;
N = 20;
A = F; % use dynamics from previous step
B1 = B * g;
for j = N:-1:1
if j == N
P = Q;
else
P = Q + A'*(P - P*B1*(R + B'*P*B1)^-1*B1'*P)*A;
end
end
K = (R + B1' * P * B1)^-1*B1'*P*A;
for i = 1:size(state,1)
u_computed(i) = -K * state(i,:)';
end
Here is the spectrum of the output with the state estimator and with the raw gyro. You can see the spectrum is lower at most frequencies (especially the peaks) with the state estimator. I'm definitely curious to measure the power consumption at a hover in the two conditions since applying a smoother signal to the motors should be more efficient
We definitely might be on to something here.
%% LQR calculation
g = 25000;
a = 0.01;
dt = 0.0015;
Q = diag([100 1 1]);
F = [1 dt 0; 0 1 dt; 0 0 1-a];
B = [0 0 a*g]';
R = 1e1;
[K,~,e] = dlqr(A,B,Q,R,zeros(size(A,1),size(B,2)))
[K,~,e] = lqrd((A-eye(3))/dt,B / dt,Q,R,dt)
Both produce similar results. E.g. 0.0508 0.0233 0.0051. The numbers translate to much higher values than my standard PID (e.g. I run rate Kp = 0.003 outer Kp = 3 which would be more like [0.009 0.003] for the first two elements). However, running it over the data produces sane results. I'm plotting the real output from the PID controller, the LQR output, and the LQR output without using the estimate of bias for state
pseudo_state = [state(:,1) state(:,2) state(:,3)+state(:,4)];
u_computed = - pseudo_state * K';
u_nt = - state(:,1:2) * K(1:2)';
ts = (t-t(1)) / 1000;
plot(ts,u_computed,ts,u_nt,ts,u)
legend('LQR','LQR_NoTorque','real')
xlabel('Time (s)');
ylabel('Output (au)')
Note that I have to create a pseudostate collapsing the bias estimate with the torque estimate to control estimated torque. Not doing this results in the output drifting away from mean zero a lot (possibly it would be more optimal but scares me for now).
It's a bit noisy to see, but the real output and the LQR coefficients without torque are pretty similar (note than depending on the values of Q and R this isn't always the case). The real LQR output drifts around a bit still (poor capture of bias?) but more interesting is always phase advanced and higher in amplitude than the real output. That phase advance is basically the same as adding in Kd, although now it is a nice principled (and lower noise) way to add it.
As a test of robustness I ran this on the data from the system identification, where there is a lot of perturbations added to the outputs that are not known to the controller or state estimator. It does quite well (and looks cleaner since it's indoors)
So it's looking maybe doable. I'm still not sure how confident I'll get in the rate estimator (or how easy it will be to estimate these terms for people without the overo) so we'll see if this becomes widely usable. It would be pretty unpleasant if the rate estimator blew up or a prop came off and the torque estimator produced nonsense.
rate_desired = outer_kp = (angle_desired - angle_current)
output = inner_kp * (rate_desired - rate_current)
= inner_kp * (outer_kp * (angle_desired - angle_current) - rate_current)
= inner_kp*outer_kp * (angle_desired - angle_current) - inner_kp * rate_current
So you can see it was tracking really well. About 150 ms of latency between control and what it achieves. You can also see the steady state error is consistently in one direction and I think this is where some integral action is required.
Any suggestions for how to try and estimate the two parameters of the KF (latency and gain)? I remember previously making an augmented state EKF for parameter estimation but I can't find any papers/chapters on the technique.
%% LQR for position
% state:
% 1. position
% 2. velocity
% 3. angle
% 4. rate
% 5. torque
%
% hovering so T*cos(angle) = 9.81
% T = 9.81 / cos(angle)
% dPosition/dt = velocity
% dVelocity/dt = -kf * Velocity + T*sin(angle) = 9.81*tan(angle) ~ 9.81 * angle * pi / 180
% dAngle/dt = rate
% dRate/dt = torque
% dTorque/dt = a*g
g = 25000;
a = 0.01;
dt = 0.0015;
kf = 0;
Q = diag([100 10 10 10 1]);
A = [1 dt 0 0 0; ...
0 (1-kf) (dt*9.81*pi/180) 0 0; ...
0 0 1 dt 0; ...
0 0 0 1 dt;
0 0 0 0 1-a];
B = [0 0 0 0 a*g]';
R = 1e5;
[K,~,e] = lqrd((A-eye(6))/dt,B / dt,Q,R,dt)
% or more precisely as a function of angle
A = @(angle) [1 dt 0 0 0; ...
0 1 (dt*9.81*tan(angle * pi / 180)/angle) 0 0; ...
0 0 1 dt 0; ...
0 0 0 1 dt;
0 0 0 0 1-a];
B = [0 0 0 0 a*g]';
R = 1e5;
angles = linspace(0.01,30,50)
for i = 1:length(angles)
angle = angles(i);
[K(i,:),~,e] = lqrd((A(angle)-eye(5))/dt,B / dt,Q,R,dt)
end
However, I don't really want to implement it as is, because for now I'd like to keep the attitude controller separate from the position controller (which outputs attitude desired). Is there a way to use the full state knowledge (and dynamics I input above) to compute the desired angle?
It sounds like what you were describing above with coupled LQR.