Lines Matching refs:w
313 void Fusion::handleGyro(const vec3_t& w, float dT) { in handleGyro() argument
314 if (!checkInitComplete(GYRO, w, dT)) in handleGyro()
317 predict(w, dT); in handleGyro()
423 F[0].x = q.w; F[1].x =-q.z; F[2].x = q.y; in getF()
424 F[0].y = q.z; F[1].y = q.w; F[2].y =-q.x; in getF()
425 F[0].z =-q.y; F[1].z = q.x; F[2].z = q.w; in getF()
426 F[0].w =-q.x; F[1].w =-q.y; F[2].w =-q.z; in getF()
430 void Fusion::predict(const vec3_t& w, float dT) { in predict() argument
433 vec3_t we = w - b; in predict()
477 O[0].xyz = O33[0]; O[0].w = -psi.x; in predict()
478 O[1].xyz = O33[1]; O[1].w = -psi.y; in predict()
479 O[2].xyz = O33[2]; O[2].w = -psi.z; in predict()
480 O[3].xyz = psi; O[3].w = k2; in predict()
487 if (x0.w < 0) in predict()
544 vec3_t w; in getOrthogonal() local
546 w[0]=0.f; in getOrthogonal()
547 w[1] = v[2]; in getOrthogonal()
548 w[2] = -v[1]; in getOrthogonal()
550 w[0] = v[2]; in getOrthogonal()
551 w[1] = 0.f; in getOrthogonal()
552 w[2] = -v[0]; in getOrthogonal()
554 w[0] = v[1]; in getOrthogonal()
555 w[1] = -v[0]; in getOrthogonal()
556 w[2] = 0.f; in getOrthogonal()
558 return normalize(w); in getOrthogonal()