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