Lines Matching refs:vec3_t
261 bool Fusion::checkInitComplete(int what, const vec3_t& d, float dT) { in checkInitComplete()
294 vec3_t up(mData[0]); in checkInitComplete()
295 vec3_t east; in checkInitComplete()
303 vec3_t north(cross_product(up, east)); in checkInitComplete()
313 void Fusion::handleGyro(const vec3_t& w, float dT) { in handleGyro()
320 status_t Fusion::handleAcc(const vec3_t& a, float dT) { in handleAcc()
334 vec3_t w_dummy; in handleAcc()
340 vec3_t m; in handleAcc()
345 vec3_t unityA = a * l_inv; in handleAcc()
353 status_t Fusion::handleMag(const vec3_t& m) { in handleMag()
370 const vec3_t up( getRotationMatrix() * Ba ); in handleMag()
371 const vec3_t east( cross_product(m, up) ); in handleMag()
383 vec3_t north( cross_product(up, east) ); in handleMag()
408 vec3_t Fusion::getBias() const { in getBias()
430 void Fusion::predict(const vec3_t& w, float dT) { in predict()
432 const vec3_t b = x1; in predict()
433 vec3_t we = w - b; in predict()
474 const vec3_t psi(sinf(hlwedT)*ilwe*we); in predict()
495 void Fusion::update(const vec3_t& z, const vec3_t& Bi, float sigma) { in update()
499 const vec3_t Bb(A*Bi); in update()
529 const vec3_t e(z - Bb); in update()
530 const vec3_t dq(K[0]*e); in update()
536 const vec3_t db(K[1]*e); in update()
543 vec3_t Fusion::getOrthogonal(const vec3_t &v) { in getOrthogonal()
544 vec3_t w; in getOrthogonal()