137 static int fusion_init_complete(struct Fusion *fusion, int what, const struct Vec3 *d, float dT) {  in fusion_init_complete()
148             struct Vec3 unityD = *d;  in fusion_init_complete()
162             struct Vec3 unityD = *d;  in fusion_init_complete()
176             struct Vec3 scaledD = *d;  in fusion_init_complete()
200         struct Vec3 up = fusion->mData[0];  in fusion_init_complete()
202         struct Vec3 east;  in fusion_init_complete()
210         struct Vec3 north;  in fusion_init_complete()
233 static void matrixCross(struct Mat33 *out, struct Vec3 *p, float diag) {  in matrixCross()
261 static void fusionPredict(struct Fusion *fusion, const struct Vec3 *w) {  in fusionPredict()
265     struct Vec3 b = fusion->x1;  in fusionPredict()
267     struct Vec3 we = *w;  in fusionPredict()
295     struct Vec3 psi = we;  in fusionPredict()
298     struct Vec3 negPsi = psi;  in fusionPredict()
389 void fusionHandleGyro(struct Fusion *fusion, const struct Vec3 *w, float dT) {  in fusionHandleGyro()
431         struct Fusion *fusion, const struct Vec3 *z, const struct Vec3 *Bi, float sigma) {  in fusionUpdate()
435     struct Vec3 Bb;  in fusionUpdate()
477     struct Vec3 e = *z;  in fusionUpdate()
480     struct Vec3 dq;  in fusionUpdate()
501         struct Vec3 db;  in fusionUpdate()
513 int fusionHandleAcc(struct Fusion *fusion, const struct Vec3 *a, float dT) {  in fusionHandleAcc()
530         struct Vec3 w_dummy;  in fusionHandleAcc()
547         struct Vec3 m;  in fusionHandleAcc()
555     struct Vec3 unityA = *a;  in fusionHandleAcc()
564             struct Vec3 aa;  in fusionHandleAcc()
590 int fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m, float dT) {  in fusionHandleMag()
607     struct Vec3 up;  in fusionHandleMag()
610     struct Vec3 east;  in fusionHandleMag()
624     struct Vec3 north;  in fusionHandleMag()
633         struct Vec3 mm;  in fusionHandleMag()
685 void fusionGetBias(const struct Fusion *fusion, struct Vec3 *bias) {  in fusionGetBias()