Lines Matching refs:vec3_t
229 bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) { in checkInitComplete()
261 vec3_t up(mData[0]); in checkInitComplete()
262 vec3_t east(cross_product(mData[1], up)); in checkInitComplete()
264 vec3_t north(cross_product(up, east)); in checkInitComplete()
274 void Fusion::handleGyro(const vec3_t& w, float dT) { in handleGyro()
281 status_t Fusion::handleAcc(const vec3_t& a) { in handleAcc()
295 status_t Fusion::handleMag(const vec3_t& m) { in handleMag()
312 const vec3_t up( getRotationMatrix() * Ba ); in handleMag()
313 const vec3_t east( cross_product(m, up) ); in handleMag()
325 vec3_t north( cross_product(up, east) ); in handleMag()
350 vec3_t Fusion::getBias() const { in getBias()
372 void Fusion::predict(const vec3_t& w, float dT) { in predict()
374 const vec3_t b = x1; in predict()
375 const vec3_t we = w - b; in predict()
413 const vec3_t psi(sinf(hlwedT)*ilwe*we); in predict()
433 void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) { in update()
437 const vec3_t Bb(A*Bi); in update()
467 const vec3_t e(z - Bb); in update()
468 const vec3_t dq(K[0]*e); in update()
469 const vec3_t db(K[1]*e); in update()