Lines Matching refs:dT
218 void Fusion::initFusion(const vec4_t& q, float dT)
229 // q00 = sv^2.dt + 1/3.su^2.dt^3
230 // q10 = q01 = 1/2.su^2.dt^2
231 // q11 = su^2.dt
234 const float dT2 = dT*dT;
235 const float dT3 = dT2*dT;
237 // variance of integrated output at 1/dT Hz (random drift)
238 const float q00 = mParam.gyroVar * dT + 0.33333f * mParam.gyroBiasVar * dT3;
241 const float q11 = mParam.gyroBiasVar * dT;
261 bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
270 mGyroRate = dT;
277 mGyroRate = dT;
278 mData[2] += d*dT;
313 void Fusion::handleGyro(const vec3_t& w, float dT) {
314 if (!checkInitComplete(GYRO, w, dT))
317 predict(w, dT);
320 status_t Fusion::handleAcc(const vec3_t& a, float dT) {
321 if (!checkInitComplete(ACC, a, dT))
336 predict(w_dummy, dT);
430 void Fusion::predict(const vec3_t& w, float dT) {
441 // O(w) = | cos(0.5*||w||*dT)*I33 - [psi]x psi |
442 // | -psi' cos(0.5*||w||*dT) |
444 // psi = sin(0.5*||w||*dT)*w / ||w||
457 // - [w]x * sin(||w||*dt)/||w||
458 // + [w]x^2 * (1-cos(||w||*dT))/||w||^2
460 // Phi10 = [w]x * (1 - cos(||w||*dt))/||w||^2
461 // - [w]x^2 * (||w||*dT - sin(||w||*dt))/||w||^3
462 // - I33*dT
465 const mat33_t I33dT(dT);
468 const float lwedT = length(we)*dT;
