void WindEKF::RungeKutta(float U[NUMU], float dT) { float dT2 = dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX]; uint8_t i; for (i = 0; i < NUMX; i++) Xlast[i] = X[i]; // make a working copy StateEq(U, K1); // k1 = f(x,u) for (i = 0; i < NUMX; i++) X[i] = Xlast[i] + dT2 * K1[i]; StateEq(U, K2); // k2 = f(x+0.5*dT*k1,u) for (i = 0; i < NUMX; i++) X[i] = Xlast[i] + dT2 * K2[i]; StateEq(U, K3); // k3 = f(x+0.5*dT*k2,u) for (i = 0; i < NUMX; i++) X[i] = Xlast[i] + dT * K3[i]; StateEq(U, K4); // k4 = f(x+dT*k3,u) // Xnew = X + dT*(k1+2*k2+2*k3+k4)/6 for (i = 0; i < NUMX; i++) X[i] = Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] + K4[i]) / 6; }
void WindEKF::RungeKutta(float U[NUMU], float dT) { // Output, Xnew, is written over X // NOTE the algorithm assumes time invariant state equations and // constant inputs over integration step float dT2 = dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX]; uint8_t i; for (i = 0; i < NUMX; i++) Xlast[i] = X[i]; // make a working copy StateEq(U, K1); // k1 = f(x,u) for (i = 0; i < NUMX; i++) X[i] = Xlast[i] + dT2 * K1[i]; StateEq(U, K2); // k2 = f(x+0.5*dT*k1,u) for (i = 0; i < NUMX; i++) X[i] = Xlast[i] + dT2 * K2[i]; StateEq(U, K3); // k3 = f(x+0.5*dT*k2,u) for (i = 0; i < NUMX; i++) X[i] = Xlast[i] + dT * K3[i]; StateEq(U, K4); // k4 = f(x+dT*k3,u) // Xnew = X + dT*(k1+2*k2+2*k3+k4)/6 for (i = 0; i < NUMX; i++) X[i] = Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] + K4[i]) / 6; }