mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
Update the altitude KF to use the updated measurements appropriately and check
in the relevant matlab code to check it and genererate it. Produces quite smooth traces.
This commit is contained in:
James Cotton
parent
fbb0b1173a
commit
7d4582e5f0
@ -120,6 +120,15 @@ float switchThrottle;
|
||||
float smoothed_altitude;
|
||||
float starting_altitude;
|
||||
|
||||
float dT;
|
||||
float z[3] = {0, 0, 0};
|
||||
float z_new[3];
|
||||
float P[3][3], K[3][2], x[2];
|
||||
float G[3] = {1.0e-5f, 1.0e-5f, 1.0e-3f};
|
||||
float S[2] = {1.0f,10.0f};
|
||||
float V[3] = {10.0f, 10.0f, 10.0f};
|
||||
|
||||
int32_t loop_count;
|
||||
/**
|
||||
* Module thread, should not return.
|
||||
*/
|
||||
@ -142,7 +151,9 @@ static void altitudeHoldTask(void *parameters)
|
||||
// Main task loop
|
||||
lastSysTime = xTaskGetTickCount();
|
||||
while (1) {
|
||||
|
||||
loop_count++;
|
||||
|
||||
bool baro_updated;
|
||||
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
|
||||
if ( xQueueReceive(queue, &ev, 100 / portTICK_RATE_MS) != pdTRUE )
|
||||
{
|
||||
@ -152,6 +163,8 @@ static void altitudeHoldTask(void *parameters)
|
||||
// Todo: Add alarm if it should be running
|
||||
continue;
|
||||
} else if (ev.obj == BaroAltitudeHandle()) {
|
||||
baro_updated = true;
|
||||
|
||||
AltHoldSmoothedData altHold;
|
||||
AltHoldSmoothedGet(&altHold);
|
||||
float dT;
|
||||
@ -222,18 +235,12 @@ static void altitudeHoldTask(void *parameters)
|
||||
} else if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD)
|
||||
running = false;
|
||||
} else if (ev.obj == AccelsHandle()) {
|
||||
float dT;
|
||||
float z[3] = {0, 0, 0};
|
||||
float P[3][3], K[3][2], x[2];
|
||||
float G[3] = {1.0e-3f, 1.0e-3f, 1.0e-3f};
|
||||
float S[2] = {0.0001f,1.0f}; //{1.0f,100.0f}; //{2.0f, 10.0f};
|
||||
float V[3] = {10.0f, 10.0f, 10.0f};
|
||||
static uint32_t timeval;
|
||||
|
||||
/* Somehow this always assigns to zero. Compiler bug? Race condition? */
|
||||
S[0] = altitudeHoldSettings.PressureNoise;
|
||||
S[1] = altitudeHoldSettings.AccelNoise;
|
||||
G[2] = altitudeHoldSettings.AccelDrift;
|
||||
//S[0] = altitudeHoldSettings.PressureNoise;
|
||||
//S[1] = altitudeHoldSettings.AccelNoise;
|
||||
//G[2] = altitudeHoldSettings.AccelDrift;
|
||||
|
||||
AccelsData accels;
|
||||
AccelsGet(&accels);
|
||||
@ -250,41 +257,64 @@ static void altitudeHoldTask(void *parameters)
|
||||
} else if (init == WAITING_BARO)
|
||||
continue;
|
||||
|
||||
//rotate avg accels into earth frame and store it
|
||||
float q[4], Rbe[3][3];
|
||||
q[0]=attitudeActual.q1;
|
||||
q[1]=attitudeActual.q2;
|
||||
q[2]=attitudeActual.q3;
|
||||
q[3]=attitudeActual.q4;
|
||||
Quaternion2R(q, Rbe);
|
||||
|
||||
x[0] = baro.Altitude;
|
||||
x[1] = -(Rbe[0][2]*accels.x+ Rbe[1][2]*accels.y + Rbe[2][2]*accels.z + 9.81f);
|
||||
//rotate avg accels into earth frame and store it
|
||||
if(1) {
|
||||
float q[4], Rbe[3][3];
|
||||
q[0]=attitudeActual.q1;
|
||||
q[1]=attitudeActual.q2;
|
||||
q[2]=attitudeActual.q3;
|
||||
q[3]=attitudeActual.q4;
|
||||
Quaternion2R(q, Rbe);
|
||||
x[1] = -(Rbe[0][2]*accels.x+ Rbe[1][2]*accels.y + Rbe[2][2]*accels.z + 9.81f);
|
||||
} else {
|
||||
x[1] = -accels.z + 9.81f;
|
||||
}
|
||||
|
||||
dT = PIOS_DELAY_DiffuS(timeval) / 1.0e6f;
|
||||
timeval = PIOS_DELAY_GetRaw();
|
||||
|
||||
V[0] = 0.014f;
|
||||
V[1] = 9.6f;
|
||||
V[2] = 3.2e-3f;
|
||||
|
||||
P[0][0] = G[0]+V[0]+(dT*dT)*V[1];
|
||||
P[0][1] = dT*V[1];
|
||||
P[1][0] = dT*V[1];
|
||||
P[1][1] = G[1]+V[1]+(dT*dT)*V[2];
|
||||
P[1][2] = dT*V[2];
|
||||
P[2][1] = dT*V[2];
|
||||
P[2][2] = G[2]+V[2];
|
||||
P[1][0] = P[0][1] = dT*V[1];
|
||||
P[2][1] = P[1][2] = dT*V[2];
|
||||
|
||||
//temp = (dT*V[1]); ///(G[0]+S[0]+V[0]+(dT*dT)*V[1]);
|
||||
K[0][0] = -S[0]/(G[0]+S[0]+V[0]+(dT*dT)*V[1])+1.0f;
|
||||
K[1][0] = (dT*V[1])/(G[0]+S[0]+V[0]+(dT*dT)*V[1]);
|
||||
K[1][1] = (dT*V[2])/(G[2]+S[1]+V[2]);
|
||||
K[2][1] = -S[1]/(G[2]+S[1]+V[2])+1.0f;
|
||||
if (baro_updated) {
|
||||
K[0][0] = -S[0]/(G[0]+S[0]+V[0]+(dT*dT)*V[1])+1.0f;
|
||||
K[1][0] = (dT*V[1])/(G[0]+S[0]+V[0]+(dT*dT)*V[1]);
|
||||
K[1][1] = (dT*V[2])/(G[2]+S[1]+V[2]);
|
||||
K[2][1] = -S[1]/(G[2]+S[1]+V[2])+1.0f;
|
||||
|
||||
z[0] = -K[0][0]*(dT*z[1]-x[0]+z[0])+dT*z[1]+K[0][1]*(x[1]-z[2])+z[0];
|
||||
z[1] = -K[1][0]*(dT*z[1]-x[0]+z[0])+dT*z[2]+K[1][1]*(x[1]-z[2])+z[1];
|
||||
z[2] = -K[2][0]*(dT*z[1]-x[0]+z[0])+K[2][1]*(x[1]-z[2])+z[2];
|
||||
z_new[0] = -K[0][0]*(dT*z[1]-x[0]+z[0])+dT*z[1]+z[0];
|
||||
z_new[1] = -K[1][0]*(dT*z[1]-x[0]+z[0])+dT*z[2]+K[1][1]*(x[1]-z[2])+z[1];
|
||||
z_new[2] = K[2][1]*(x[1]-z[2])+z[2];
|
||||
|
||||
V[0] = -K[0][1]*P[2][0]-P[0][0]*(K[0][0]-1.0f);
|
||||
V[1] = P[1][1]-K[1][0]*P[0][1]-K[1][1]*P[2][1];
|
||||
V[2] = -K[2][0]*P[0][2]-P[2][2]*(K[2][1]-1.0f);
|
||||
memcpy(z, z_new, sizeof(z_new));
|
||||
|
||||
V[0] = -P[0][0]*(K[0][0]-1.0f);
|
||||
V[1] = P[1][1]-K[1][0]*P[0][1]-K[1][1]*P[2][1];
|
||||
V[2] = -P[2][2]*(K[2][1]-1.0f);
|
||||
|
||||
baro_updated = false;
|
||||
} else {
|
||||
K[1][0] = (dT*V[2])/(G[2]+S[1]+V[2]);
|
||||
K[2][0] = -S[1]/(G[2]+S[1]+V[2])+1.0f;
|
||||
|
||||
z_new[0] = dT*z[1]+z[0];
|
||||
z_new[1] = dT*z[2]+K[1][0]*(x[1]-z[2])+z[1];
|
||||
z_new[2] = K[2][0]*(x[1]-z[2])+z[2];
|
||||
|
||||
memcpy(z, z_new, sizeof(z_new));
|
||||
|
||||
V[0] = P[0][0];
|
||||
V[1] = P[1][1]-K[1][0]*P[2][1];
|
||||
V[2] = -P[2][2]*(K[2][0]-1.0f);
|
||||
}
|
||||
|
||||
AltHoldSmoothedData altHold;
|
||||
AltHoldSmoothedGet(&altHold);
|
||||
|
||||
108
matlab/revo/async_attitude.m
Normal file
108
matlab/revo/async_attitude.m
Normal file
@ -0,0 +1,108 @@
|
||||
baro = fixTime(BaroAltitude);
|
||||
accel = fixTime(Accels);
|
||||
attitude = fixTime(AttitudeActual);
|
||||
|
||||
|
||||
Gamma = diag([1e-5 1e-5 0.00001 1e-20]); % state noise
|
||||
accel_sigma = 1;
|
||||
baro_sigma = 1;
|
||||
Nu = diag([10 10 10 10]);
|
||||
z = zeros(length(accel.z),4);
|
||||
Nu_n = zeros([4 4 length(accel.z)]);
|
||||
Nu_n(:,:,1) = Nu;
|
||||
|
||||
t = max(accel.timestamp(1), baro.timestamp(1));
|
||||
last_t = t-1;
|
||||
last_accel_idx = 1;
|
||||
last_baro_idx = 1;
|
||||
i = 1;
|
||||
timestamp = [];
|
||||
|
||||
z(1) = baro.Altitude(1);
|
||||
z(1:5,4) = 0.4;
|
||||
timestamp(1) = t;
|
||||
log_accel = 0;
|
||||
while (last_accel_idx + 1) <= length(accel.z) && (last_baro_idx + 1) <= length(baro.Altitude)
|
||||
|
||||
update_baro = baro.timestamp(last_baro_idx + 1) < t;
|
||||
update_accel = accel.timestamp(last_accel_idx + 1) < t;
|
||||
|
||||
if 0 && update_accel
|
||||
[~,idx] = min(abs(attitude.timestamp - accel.timestamp(last_accel_idx+1)));
|
||||
Rbe = Quat2Rbe([attitude.q1(idx), attitude.q2(idx), attitude.q3(idx), attitude.q4(idx)]);
|
||||
idx = last_accel_idx + 1;
|
||||
accel_ned = Rbe * [accel.x(idx); accel.y(idx); accel.z(idx)];
|
||||
accel_ned = accel_ned(3);
|
||||
% if(abs(accel_ned) < 1e-1)
|
||||
% keyboard
|
||||
% end
|
||||
else
|
||||
accel_ned = accel.z(last_accel_idx + 1);
|
||||
end
|
||||
log_accel(i) = accel_ned;
|
||||
if update_baro && update_accel;
|
||||
x = [baro.Altitude(last_baro_idx + 1); -accel_ned-9.81];
|
||||
last_baro_idx = last_baro_idx + 1;
|
||||
last_accel_idx = last_accel_idx + 1;
|
||||
C = [1 0 0 0; 0 0 1 -1];
|
||||
Sigma = diag([baro_sigma; accel_sigma]);
|
||||
elseif update_accel
|
||||
x = -accel_ned - 9.81;
|
||||
last_accel_idx = last_accel_idx + 1;
|
||||
C = [0 0 1 -1];
|
||||
Sigma = [accel_sigma];
|
||||
elseif update_baro
|
||||
x = [baro.Altitude(last_baro_idx + 1)];
|
||||
last_baro_idx = last_baro_idx + 1;
|
||||
C = [1 0 0 0];
|
||||
Sigma = [baro_sigma];
|
||||
else
|
||||
% Take a timestep and look for advance
|
||||
t = t + 0.1;
|
||||
continue;
|
||||
end
|
||||
%[last_baro_idx last_accel_idx]
|
||||
t = max(baro.timestamp(last_baro_idx), accel.timestamp(last_accel_idx));
|
||||
dT = (t - last_t) / 1000;
|
||||
if(dT == 0)
|
||||
dT = 1.5 / 1000;
|
||||
end
|
||||
assert(dT ~= 0,'WTF');
|
||||
last_t = t;
|
||||
|
||||
i = i + 1;
|
||||
|
||||
% Zero out non-diag covariance
|
||||
Nu = diag(diag(Nu));
|
||||
|
||||
A = [1 dT 0 0; 0 1 dT 0; 0 0 1 0; 0 0 0 1];
|
||||
|
||||
P = A * Nu * A' + Gamma;
|
||||
K = P*C'*(C*P*C'+Sigma)^-1;
|
||||
|
||||
z(i,:) = A * z(i-1,:)' + K * (x - C * A * z(i-1,:)');
|
||||
timestamp(i) = t;
|
||||
Nu = (eye(4) - K * C) * P;
|
||||
Nu_n(:,:,i) = Nu;
|
||||
|
||||
z(i,4) = 0;
|
||||
if mod(i,10000) == 0
|
||||
subplot(311)
|
||||
plot(baro.timestamp, baro.Altitude, '.', timestamp(1:i),z(1:i,1),'r','LineWidth',5)
|
||||
subplot(312)
|
||||
plot(timestamp(1:i),z(1:i,2),'k')
|
||||
subplot(313)
|
||||
plot(timestamp(1:i),z(1:i,3),'k');
|
||||
xlim(timestamp([1,i]))
|
||||
drawnow
|
||||
end
|
||||
end
|
||||
|
||||
subplot(311)
|
||||
plot(baro.timestamp, baro.Altitude, '.', timestamp(1:i),z(1:i,1),'r','LineWidth',5)
|
||||
subplot(312)
|
||||
plot(timestamp(1:i),z(1:i,2),'k')
|
||||
subplot(313)
|
||||
plot(timestamp(1:i),z(1:i,3),'k');
|
||||
xlim(timestamp([1,i]))
|
||||
|
||||
61
matlab/revo/generate_altitude.m
Normal file
61
matlab/revo/generate_altitude.m
Normal file
@ -0,0 +1,61 @@
|
||||
% Generate the symbolic code for the kalman filter on altitude
|
||||
|
||||
dT = sym('dT','real');
|
||||
A = [1 dT 0; 0 1 dT; 0 0 1];
|
||||
Nu = diag([sym('V[1]') sym('V[2]') sym('V[3]')]);
|
||||
|
||||
Gamma = diag([sym('G[1]') sym('G[2]') sym('G[3]')]);
|
||||
Sigma = diag([sym('S[1]') sym('S[2]')]);
|
||||
C = [1 0 0; 0 0 1];
|
||||
state = [sym('z[1]'); sym('z[2]'); sym('z[3]')];
|
||||
measure = [sym('x[1]'); sym('x[2]')];
|
||||
|
||||
P = simplify(A * Nu * A' + Gamma);
|
||||
K = simplify(P*C'*(C*P*C'+Sigma)^-1);
|
||||
|
||||
P_mat = [sym('P[1][1]') sym('P[1][2]') sym('P[1][3]'); ...
|
||||
sym('P[2][1]') sym('P[2][2]') sym('P[2][3]'); ...
|
||||
sym('P[3][1]') sym('P[3][2]') sym('P[3][3]')];
|
||||
K_mat = [sym('K[1][1]') sym('K[1][2]'); ...
|
||||
sym('K[2][1]') sym('K[2][2]'); ...
|
||||
sym('K[3][1]') sym('K[3][2]')];
|
||||
|
||||
z_new = A * state + K_mat * (measure - C * A * state);
|
||||
V = (eye(3) - K_mat * C) * P_mat;
|
||||
|
||||
ccode(P)
|
||||
ccode(K)
|
||||
ccode(z_new)
|
||||
ccode(V)
|
||||
|
||||
|
||||
%% For when there is no baro update
|
||||
% Generate the symbolic code for the kalman filter on altitude
|
||||
|
||||
dT = sym('dT','real');
|
||||
A = [1 dT 0; 0 1 dT; 0 0 1];
|
||||
Nu = diag([sym('V[1]') sym('V[2]') sym('V[3]')]);
|
||||
|
||||
Gamma = diag([sym('G[1]') sym('G[2]') sym('G[3]')]);
|
||||
Sigma = sym('S[2]');
|
||||
C = [0 0 1];
|
||||
state = [sym('z[1]'); sym('z[2]'); sym('z[3]')];
|
||||
measure = [sym('x[2]')];
|
||||
|
||||
P = simplify(A * Nu * A' + Gamma);
|
||||
K = simplify(P*C'*(C*P*C'+Sigma)^-1);
|
||||
|
||||
P_mat = [sym('P[1][1]') sym('P[1][2]') sym('P[1][3]'); ...
|
||||
sym('P[2][1]') sym('P[2][2]') sym('P[2][3]'); ...
|
||||
sym('P[3][1]') sym('P[3][2]') sym('P[3][3]')];
|
||||
K_mat = [sym('K[1][1]'); ...
|
||||
sym('K[2][1]'); ...
|
||||
sym('K[3][1]')];
|
||||
|
||||
z_new = A * state + K_mat * (measure - C * A * state);
|
||||
V = (eye(3) - K_mat * C) * P_mat;
|
||||
|
||||
ccode(P)
|
||||
ccode(K)
|
||||
ccode(z_new)
|
||||
ccode(V)
|
||||
Loading…
Reference in New Issue
Block a user