- u =(GPS.ground_speed / 100)*(1+DCM_Matrix[3][1];
- v = 0.0;
- w = (GPS.ground_speed / 100)*-DCM_Matrix[3][1];
- p = Omega[0];
- q = Omega[1];
- r = Omega[2];
- Accel_Vector[0] += Accel_Scale(-q*w + r*v);
- Accel_Vector[1] += Accel_Scale(-r*u + p*w);
- Accel_Vector[2] += Accel_Scale(-p*v + q*u);