diff --git a/flight/Modules/AltitudeHold/altitudehold.c b/flight/Modules/AltitudeHold/altitudehold.c index a67775c9d0d..1e7b256a6c3 100644 --- a/flight/Modules/AltitudeHold/altitudehold.c +++ b/flight/Modules/AltitudeHold/altitudehold.c @@ -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]; + + 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_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]; + + memcpy(z, z_new, sizeof(z_new)); - //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; + 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); - 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]; + 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; - 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); + 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); diff --git a/matlab/revo/async_attitude.m b/matlab/revo/async_attitude.m new file mode 100644 index 00000000000..658575b5ef5 --- /dev/null +++ b/matlab/revo/async_attitude.m @@ -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])) + diff --git a/matlab/revo/generate_altitude.m b/matlab/revo/generate_altitude.m new file mode 100644 index 00000000000..09ac5ae1b25 --- /dev/null +++ b/matlab/revo/generate_altitude.m @@ -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)