Lines Matching refs:dT
188 void Fusion::initFusion(const vec4_t& q, float dT)
204 const float dT2 = dT*dT;
205 const float dT3 = dT2*dT;
207 // variance of integrated output at 1/dT Hz (random drift)
208 const float q00 = gyroVAR * dT + 0.33333f * biasVAR * dT3;
211 const float q11 = biasVAR * dT;
245 bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) {
258 mGyroRate = dT;
259 mData[2] += d*dT;
290 void Fusion::handleGyro(const vec3_t& w, float dT) {
291 if (!checkInitComplete(GYRO, w, dT))
294 predict(w, dT);
388 void Fusion::predict(const vec3_t& w, float dT) {
396 // O(w) = | cos(0.5*||w||*dT)*I33 - [psi]x psi |
397 // | -psi' cos(0.5*||w||*dT) |
399 // psi = sin(0.5*||w||*dT)*w / ||w||
413 // + [w]x^2 * (1-cos(||w||*dT))/||w||^2
416 // - [w]x^2 * (||w||*dT - sin(||w||*dt))/||w||^3
417 // - I33*dT
420 const mat33_t I33dT(dT);
423 const float lwedT = length(we)*dT;