114986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi/* 214986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * Copyright (C) 2016 The Android Open Source Project 314986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * 414986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * Licensed under the Apache License, Version 2.0 (the "License"); 514986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * you may not use this file except in compliance with the License. 614986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * You may obtain a copy of the License at 714986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * 814986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * http://www.apache.org/licenses/LICENSE-2.0 914986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * 1014986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * Unless required by applicable law or agreed to in writing, software 1114986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * distributed under the License is distributed on an "AS IS" BASIS, 1214986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 1314986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * See the License for the specific language governing permissions and 1414986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi * limitations under the License. 1514986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi */ 1614986b090cf259d3d9e6762520dfc276e30de911Ashutosh Joshi 1759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian// adapted from frameworks/native/services/sensorservice/Fusion.cpp 1859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 1939bac2d9ad663f0d040aa2e9ba0c3b51831c9dd7Meng-hsuan Chung#include <algos/fusion.h> 2059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 2159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#include <errno.h> 2259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#include <nanohub_math.h> 2359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#include <stdio.h> 2459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 2559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#include <seos.h> 2659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 2759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#ifdef DEBUG_CH 2859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian// change to 0 to disable fusion debugging output 2959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#define DEBUG_FUSION 0 3059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#endif 3159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 3259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#define ACC 1 3359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#define MAG 2 3459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#define GYRO 4 3559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 36a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian#define DEFAULT_GYRO_VAR 1e-7f 37a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian#define DEFAULT_GYRO_BIAS_VAR 1e-12f 38a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian#define DEFAULT_ACC_STDEV 5e-2f 39a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian#define DEFAULT_MAG_STDEV 5e-1f 4059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 41a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian#define GEOMAG_GYRO_VAR 2e-4f 42a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian#define GEOMAG_GYRO_BIAS_VAR 1e-4f 43a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian#define GEOMAG_ACC_STDEV 0.02f 44a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian#define GEOMAG_MAG_STDEV 0.02f 4559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 4659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#define SYMMETRY_TOLERANCE 1e-10f 4759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#define FAKE_MAG_INTERVAL 1.0f //sec 4859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 4959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#define NOMINAL_GRAVITY 9.81f 5059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#define FREE_FALL_THRESHOLD (0.1f * NOMINAL_GRAVITY) 5159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#define FREE_FALL_THRESHOLD_SQ (FREE_FALL_THRESHOLD * FREE_FALL_THRESHOLD) 5259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 53a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian#define MAX_VALID_MAGNETIC_FIELD 75.0f 5459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#define MAX_VALID_MAGNETIC_FIELD_SQ (MAX_VALID_MAGNETIC_FIELD * MAX_VALID_MAGNETIC_FIELD) 5559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 566ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu#define MIN_VALID_MAGNETIC_FIELD 20.0f //norminal mag field strength is 25uT in some area 5759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#define MIN_VALID_MAGNETIC_FIELD_SQ (MIN_VALID_MAGNETIC_FIELD * MIN_VALID_MAGNETIC_FIELD) 5859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 5959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#define MIN_VALID_CROSS_PRODUCT_MAG 1.0e-3 6059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#define MIN_VALID_CROSS_PRODUCT_MAG_SQ (MIN_VALID_CROSS_PRODUCT_MAG * MIN_VALID_CROSS_PRODUCT_MAG) 6159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 62a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian#define DELTA_TIME_MARGIN 1.0e-9f 63a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian 646ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu#define TRUST_DURATION_MANUAL_MAG_CAL 5.f //unit: seconds 656ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu 6659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qianvoid initFusion(struct Fusion *fusion, uint32_t flags) { 6759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->flags = flags; 6859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 6959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (flags & FUSION_USE_GYRO) { 7059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian // normal fusion mode 7159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->param.gyro_var = DEFAULT_GYRO_VAR; 7259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->param.gyro_bias_var = DEFAULT_GYRO_BIAS_VAR; 7359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->param.acc_stdev = DEFAULT_ACC_STDEV; 7459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->param.mag_stdev = DEFAULT_MAG_STDEV; 7559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } else { 7659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian // geo mag mode 7759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->param.gyro_var = GEOMAG_GYRO_VAR; 7859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->param.gyro_bias_var = GEOMAG_GYRO_BIAS_VAR; 7959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->param.acc_stdev = GEOMAG_ACC_STDEV; 8059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->param.mag_stdev = GEOMAG_MAG_STDEV; 8159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 8259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 8359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (flags & FUSION_REINITIALIZE) 8459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian { 8559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initVec3(&fusion->Ba, 0.0f, 0.0f, 1.0f); 8659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initVec3(&fusion->Bm, 0.0f, 1.0f, 0.0f); 8759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 8859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initVec4(&fusion->x0, 0.0f, 0.0f, 0.0f, 0.0f); 8959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initVec3(&fusion->x1, 0.0f, 0.0f, 0.0f); 9059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 9159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->mInitState = 0; 9259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 93a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian fusion->mPredictDt = 0.0f; 9459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->mCount[0] = fusion->mCount[1] = fusion->mCount[2] = 0; 9559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 9659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initVec3(&fusion->mData[0], 0.0f, 0.0f, 0.0f); 9759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initVec3(&fusion->mData[1], 0.0f, 0.0f, 0.0f); 9859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initVec3(&fusion->mData[2], 0.0f, 0.0f, 0.0f); 996ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu 10059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } else { 10159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian // mask off disabled sensor bit 10259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->mInitState &= (ACC 10359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian | ((fusion->flags & FUSION_USE_MAG) ? MAG : 0) 10459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian | ((fusion->flags & FUSION_USE_GYRO) ? GYRO : 0)); 10559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 1066ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu 1076ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusionSetMagTrust(fusion, NORMAL); 1086ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusion->lastMagInvalid = false; 10959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 11059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 11159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qianint fusionHasEstimate(const struct Fusion *fusion) { 11259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian // waive sensor init depends on the mode 11359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian return fusion->mInitState == (ACC 11459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian | ((fusion->flags & FUSION_USE_MAG) ? MAG : 0) 11559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian | ((fusion->flags & FUSION_USE_GYRO) ? GYRO : 0)); 11659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 11759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 118a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qianstatic void updateDt(struct Fusion *fusion, float dT) { 119a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian if (fabsf(fusion->mPredictDt - dT) > DELTA_TIME_MARGIN) { 120a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian float dT2 = dT * dT; 121a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian float dT3 = dT2 * dT; 122a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian 123a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian float q00 = fusion->param.gyro_var * dT + 124a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian 0.33333f * fusion->param.gyro_bias_var * dT3; 125a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian float q11 = fusion->param.gyro_bias_var * dT; 126a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian float q10 = 0.5f * fusion->param.gyro_bias_var * dT2; 127a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian float q01 = q10; 128a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian 129a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian initDiagonalMatrix(&fusion->GQGt[0][0], q00); 130a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian initDiagonalMatrix(&fusion->GQGt[0][1], -q10); 131a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian initDiagonalMatrix(&fusion->GQGt[1][0], -q01); 132a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian initDiagonalMatrix(&fusion->GQGt[1][1], q11); 133a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian fusion->mPredictDt = dT; 134a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian } 13559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 13659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 13759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qianstatic int fusion_init_complete(struct Fusion *fusion, int what, const struct Vec3 *d, float dT) { 13859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (fusionHasEstimate(fusion)) { 13959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian return 1; 14059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 14159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 14259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian switch (what) { 14359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian case ACC: 14459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian { 14559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (!(fusion->flags & FUSION_USE_GYRO)) { 146a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian updateDt(fusion, dT); 14759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 14859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 unityD = *d; 14959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3Normalize(&unityD); 15059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 15159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3Add(&fusion->mData[0], &unityD); 15259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian ++fusion->mCount[0]; 15359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 154e80032b41b6614389164c807d9c28f7298535e00Zhengyin Qian if (fusion->mCount[0] == 8) { 15559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->mInitState |= ACC; 15659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 15759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian break; 15859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 15959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 16059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian case MAG: 16159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian { 16259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 unityD = *d; 16359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3Normalize(&unityD); 16459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 16559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3Add(&fusion->mData[1], &unityD); 16659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian ++fusion->mCount[1]; 16759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 16859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->mInitState |= MAG; 16959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian break; 17059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 17159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 17259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian case GYRO: 17359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian { 174a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian updateDt(fusion, dT); 17559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 17659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 scaledD = *d; 17759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3ScalarMul(&scaledD, dT); 17859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 17959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3Add(&fusion->mData[2], &scaledD); 18059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian ++fusion->mCount[2]; 18159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 18259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->mInitState |= GYRO; 18359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian break; 18459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 18559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 18659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian default: 18759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian // assert(!"should not be here"); 18859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian break; 18959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 19059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 19159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (fusionHasEstimate(fusion)) { 19259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3ScalarMul(&fusion->mData[0], 1.0f / fusion->mCount[0]); 19359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 19459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (fusion->flags & FUSION_USE_MAG) { 19559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3ScalarMul(&fusion->mData[1], 1.0f / fusion->mCount[1]); 196e80032b41b6614389164c807d9c28f7298535e00Zhengyin Qian } else { 197e80032b41b6614389164c807d9c28f7298535e00Zhengyin Qian fusion->fake_mag_decimation = 0.f; 19859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 19959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 20059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 up = fusion->mData[0]; 20159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 20259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 east; 20359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (fusion->flags & FUSION_USE_MAG) { 20459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3Cross(&east, &fusion->mData[1], &up); 20559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3Normalize(&east); 20659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } else { 20759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian findOrthogonalVector(up.x, up.y, up.z, &east.x, &east.y, &east.z); 20859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 20959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 21059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 north; 21159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3Cross(&north, &up, &east); 21259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 21359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 R; 21459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initMatrixColumns(&R, &east, &north, &up); 21559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 216a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian //Quat q; 217a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian //initQuat(&q, &R); 218a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian 219a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian initQuat(&fusion->x0, &R); 220a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian initVec3(&fusion->x1, 0.0f, 0.0f, 0.0f); 22159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 222a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian initZeroMatrix(&fusion->P[0][0]); 223a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian initZeroMatrix(&fusion->P[0][1]); 224a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian initZeroMatrix(&fusion->P[1][0]); 225a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian initZeroMatrix(&fusion->P[1][1]); 2266ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu 2276ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusionSetMagTrust(fusion, INITIALIZATION); 22859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 22959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 23059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian return 0; 23159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 23259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 23359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qianstatic void matrixCross(struct Mat33 *out, struct Vec3 *p, float diag) { 23459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian out->elem[0][0] = diag; 23559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian out->elem[1][1] = diag; 23659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian out->elem[2][2] = diag; 23759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian out->elem[1][0] = p->z; 23859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian out->elem[0][1] = -p->z; 23959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian out->elem[2][0] = -p->y; 24059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian out->elem[0][2] = p->y; 24159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian out->elem[2][1] = p->x; 24259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian out->elem[1][2] = -p->x; 24359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 24459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 24559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qianstatic void fusionCheckState(struct Fusion *fusion) { 24659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 24759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (!mat33IsPositiveSemidefinite(&fusion->P[0][0], SYMMETRY_TOLERANCE) 24859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian || !mat33IsPositiveSemidefinite( 24959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian &fusion->P[1][1], SYMMETRY_TOLERANCE)) { 25059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 25159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initZeroMatrix(&fusion->P[0][0]); 25259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initZeroMatrix(&fusion->P[0][1]); 25359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initZeroMatrix(&fusion->P[1][0]); 25459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initZeroMatrix(&fusion->P[1][1]); 25559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 25659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 25759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 25859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian#define kEps 1.0E-4f 25959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 260c787e68e74e760ebaff97658797322426f9d4425Dmitry GrinbergUNROLLED 261a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qianstatic void fusionPredict(struct Fusion *fusion, const struct Vec3 *w) { 262a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian const float dT = fusion->mPredictDt; 263a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian 26459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian Quat q = fusion->x0; 26559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 b = fusion->x1; 26659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 26759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 we = *w; 26859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3Sub(&we, &b); 26959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 27059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 I33; 27159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initDiagonalMatrix(&I33, 1.0f); 27259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 27359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 I33dT; 27459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initDiagonalMatrix(&I33dT, dT); 27559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 27659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 wx; 27759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian matrixCross(&wx, &we, 0.0f); 27859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 27959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 wx2; 28059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Multiply(&wx2, &wx, &wx); 28159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 28259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian float norm_we = vec3Norm(&we); 28359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 28459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (fabsf(norm_we) < kEps) { 28559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian return; 28659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 28759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 28859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian float lwedT = norm_we * dT; 28959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian float hlwedT = 0.5f * lwedT; 29059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian float ilwe = 1.0f / norm_we; 29159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian float k0 = (1.0f - cosf(lwedT)) * (ilwe * ilwe); 29259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian float k1 = sinf(lwedT); 29359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian float k2 = cosf(hlwedT); 29459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 29559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 psi = we; 29659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3ScalarMul(&psi, sinf(hlwedT) * ilwe); 29759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 29859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 negPsi = psi; 29959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3ScalarMul(&negPsi, -1.0f); 30059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 30159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 O33; 30259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian matrixCross(&O33, &negPsi, k2); 30359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 30459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat44 O; 305c787e68e74e760ebaff97658797322426f9d4425Dmitry Grinberg uint32_t i; 30659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian for (i = 0; i < 3; ++i) { 307c787e68e74e760ebaff97658797322426f9d4425Dmitry Grinberg uint32_t j; 30859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian for (j = 0; j < 3; ++j) { 30959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian O.elem[i][j] = O33.elem[i][j]; 31059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 31159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 31259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 31359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian O.elem[3][0] = -psi.x; 31459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian O.elem[3][1] = -psi.y; 31559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian O.elem[3][2] = -psi.z; 31659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian O.elem[3][3] = k2; 31759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 31859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian O.elem[0][3] = psi.x; 31959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian O.elem[1][3] = psi.y; 32059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian O.elem[2][3] = psi.z; 32159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 32259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 tmp = wx; 32359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33ScalarMul(&tmp, k1 * ilwe); 32459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 32559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->Phi0[0] = I33; 32659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Sub(&fusion->Phi0[0], &tmp); 32759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 32859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian tmp = wx2; 32959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33ScalarMul(&tmp, k0); 33059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 33159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Add(&fusion->Phi0[0], &tmp); 33259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 33359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian tmp = wx; 33459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33ScalarMul(&tmp, k0); 33559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->Phi0[1] = tmp; 33659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 33759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Sub(&fusion->Phi0[1], &I33dT); 33859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 33959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian tmp = wx2; 34059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33ScalarMul(&tmp, ilwe * ilwe * ilwe * (lwedT - k1)); 34159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 34259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Sub(&fusion->Phi0[1], &tmp); 34359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 34459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat44Apply(&fusion->x0, &O, &q); 34559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 34659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (fusion->x0.w < 0.0f) { 34759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->x0.x = -fusion->x0.x; 34859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->x0.y = -fusion->x0.y; 34959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->x0.z = -fusion->x0.z; 35059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->x0.w = -fusion->x0.w; 35159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 35259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 35359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian // Pnew = Phi * P 35459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 35559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 Pnew[2][2]; 35659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Multiply(&Pnew[0][0], &fusion->Phi0[0], &fusion->P[0][0]); 35759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Multiply(&tmp, &fusion->Phi0[1], &fusion->P[1][0]); 35859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Add(&Pnew[0][0], &tmp); 35959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 36059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Multiply(&Pnew[0][1], &fusion->Phi0[0], &fusion->P[0][1]); 36159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Multiply(&tmp, &fusion->Phi0[1], &fusion->P[1][1]); 36259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Add(&Pnew[0][1], &tmp); 36359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 36459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian Pnew[1][0] = fusion->P[1][0]; 36559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian Pnew[1][1] = fusion->P[1][1]; 36659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 36759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian // P = Pnew * Phi^T 36859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 36959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33MultiplyTransposed2(&fusion->P[0][0], &Pnew[0][0], &fusion->Phi0[0]); 37059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33MultiplyTransposed2(&tmp, &Pnew[0][1], &fusion->Phi0[1]); 37159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Add(&fusion->P[0][0], &tmp); 37259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 37359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->P[0][1] = Pnew[0][1]; 37459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 37559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33MultiplyTransposed2(&fusion->P[1][0], &Pnew[1][0], &fusion->Phi0[0]); 37659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33MultiplyTransposed2(&tmp, &Pnew[1][1], &fusion->Phi0[1]); 37759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Add(&fusion->P[1][0], &tmp); 37859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 37959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->P[1][1] = Pnew[1][1]; 38059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 38159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Add(&fusion->P[0][0], &fusion->GQGt[0][0]); 38259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Add(&fusion->P[0][1], &fusion->GQGt[0][1]); 38359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Add(&fusion->P[1][0], &fusion->GQGt[1][0]); 38459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Add(&fusion->P[1][1], &fusion->GQGt[1][1]); 38559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 38659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusionCheckState(fusion); 38759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 38859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 38959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qianvoid fusionHandleGyro(struct Fusion *fusion, const struct Vec3 *w, float dT) { 39059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (!fusion_init_complete(fusion, GYRO, w, dT)) { 39159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian return; 39259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 39359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 394a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian updateDt(fusion, dT); 395a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian 396a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian fusionPredict(fusion, w); 39759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 39859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 399c787e68e74e760ebaff97658797322426f9d4425Dmitry GrinbergUNROLLED 40059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qianstatic void scaleCovariance(struct Mat33 *out, const struct Mat33 *A, const struct Mat33 *P) { 401c787e68e74e760ebaff97658797322426f9d4425Dmitry Grinberg uint32_t r; 40259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian for (r = 0; r < 3; ++r) { 403c787e68e74e760ebaff97658797322426f9d4425Dmitry Grinberg uint32_t j; 40459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian for (j = r; j < 3; ++j) { 40559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian float apat = 0.0f; 406c787e68e74e760ebaff97658797322426f9d4425Dmitry Grinberg uint32_t c; 40759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian for (c = 0; c < 3; ++c) { 40859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian float v = A->elem[c][r] * P->elem[c][c] * 0.5f; 409c787e68e74e760ebaff97658797322426f9d4425Dmitry Grinberg uint32_t k; 41059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian for (k = c + 1; k < 3; ++k) { 41159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian v += A->elem[k][r] * P->elem[c][k]; 41259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 41359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 41459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian apat += 2.0f * v * A->elem[c][j]; 41559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 41659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 41759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian out->elem[r][j] = apat; 41859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian out->elem[j][r] = apat; 41959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 42059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 42159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 42259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 42359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qianstatic void getF(struct Vec4 F[3], const struct Vec4 *q) { 42459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian F[0].x = q->w; F[1].x = -q->z; F[2].x = q->y; 42559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian F[0].y = q->z; F[1].y = q->w; F[2].y = -q->x; 42659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian F[0].z = -q->y; F[1].z = q->x; F[2].z = q->w; 42759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian F[0].w = -q->x; F[1].w = -q->y; F[2].w = -q->z; 42859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 42959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 43059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qianstatic void fusionUpdate( 43159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Fusion *fusion, const struct Vec3 *z, const struct Vec3 *Bi, float sigma) { 43259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 A; 43359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian quatToMatrix(&A, &fusion->x0); 43459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 43559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 Bb; 43659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Apply(&Bb, &A, Bi); 43759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 43859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 L; 43959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian matrixCross(&L, &Bb, 0.0f); 44059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 44159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 R; 44259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initDiagonalMatrix(&R, sigma * sigma); 44359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 44459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 S; 44559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian scaleCovariance(&S, &L, &fusion->P[0][0]); 44659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 44759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Add(&S, &R); 44859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 44959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 Si; 45059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Invert(&Si, &S); 45159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 45259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 LtSi; 45359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33MultiplyTransposed(&LtSi, &L, &Si); 45459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 45559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 K[2]; 45659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Multiply(&K[0], &fusion->P[0][0], &LtSi); 45759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33MultiplyTransposed(&K[1], &fusion->P[0][1], &LtSi); 45859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 45959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 K0L; 46059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Multiply(&K0L, &K[0], &L); 46159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 46259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 K1L; 46359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Multiply(&K1L, &K[1], &L); 46459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 46559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 tmp; 46659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Multiply(&tmp, &K0L, &fusion->P[0][0]); 46759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Sub(&fusion->P[0][0], &tmp); 46859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 46959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Multiply(&tmp, &K1L, &fusion->P[0][1]); 47059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Sub(&fusion->P[1][1], &tmp); 47159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 47259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Multiply(&tmp, &K0L, &fusion->P[0][1]); 47359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Sub(&fusion->P[0][1], &tmp); 47459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 47559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Transpose(&fusion->P[1][0], &fusion->P[0][1]); 47659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 47759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 e = *z; 47859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3Sub(&e, &Bb); 47959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 48059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 dq; 48159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Apply(&dq, &K[0], &e); 48259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 48359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 48459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec4 F[3]; 48559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian getF(F, &fusion->x0); 48659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 48759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian // 4x3 * 3x1 => 4x1 48859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 48959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec4 q; 49059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian q.x = fusion->x0.x + 0.5f * (F[0].x * dq.x + F[1].x * dq.y + F[2].x * dq.z); 49159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian q.y = fusion->x0.y + 0.5f * (F[0].y * dq.x + F[1].y * dq.y + F[2].y * dq.z); 49259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian q.z = fusion->x0.z + 0.5f * (F[0].z * dq.x + F[1].z * dq.y + F[2].z * dq.z); 49359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian q.w = fusion->x0.w + 0.5f * (F[0].w * dq.x + F[1].w * dq.y + F[2].w * dq.z); 49459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 49559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->x0 = q; 49659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian quatNormalize(&fusion->x0); 49759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 49859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (fusion->flags & FUSION_USE_MAG) { 49959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian // accumulate gyro bias (causes self spin) only if not 50059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian // game rotation vector 50159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 db; 50259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Apply(&db, &K[1], &e); 50359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3Add(&fusion->x1, &db); 50459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 50559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 50659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusionCheckState(fusion); 50759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 50859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 509a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian#define ACC_TRUSTWORTHY(abs_norm_err) ((abs_norm_err) < 1.f) 510a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian#define ACC_COS_CONV_FACTOR 0.01f 511a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian#define ACC_COS_CONV_LIMIT 3.f 512a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian 51359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qianint fusionHandleAcc(struct Fusion *fusion, const struct Vec3 *a, float dT) { 51459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (!fusion_init_complete(fusion, ACC, a, dT)) { 51559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian return -EINVAL; 51659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 51759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 51859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian float norm2 = vec3NormSquared(a); 51959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 52059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (norm2 < FREE_FALL_THRESHOLD_SQ) { 52159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian return -EINVAL; 52259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 52359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 52459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian float l = sqrtf(norm2); 52559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian float l_inv = 1.0f / l; 52659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 52759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (!(fusion->flags & FUSION_USE_GYRO)) { 52859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian // geo mag mode 52959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian // drive the Kalman filter with zero mean dummy gyro vector 53059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 w_dummy; 53159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 53259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian // avoid (fabsf(norm_we) < kEps) in fusionPredict() 53359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian initVec3(&w_dummy, fusion->x1.x + kEps, fusion->x1.y + kEps, 53459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->x1.z + kEps); 535a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian 536a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian updateDt(fusion, dT); 537a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian fusionPredict(fusion, &w_dummy); 53859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 53959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 540a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian struct Mat33 R; 541a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian fusionGetRotationMatrix(fusion, &R); 542a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian 54359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (!(fusion->flags & FUSION_USE_MAG) && 544e80032b41b6614389164c807d9c28f7298535e00Zhengyin Qian (fusion->fake_mag_decimation += dT) > FAKE_MAG_INTERVAL) { 54559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian // game rotation mode, provide fake mag update to prevent 54659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian // P to diverge over time 54759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 m; 54859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Apply(&m, &R, &fusion->Bm); 54959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 55059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusionUpdate(fusion, &m, &fusion->Bm, 55159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusion->param.mag_stdev); 552e80032b41b6614389164c807d9c28f7298535e00Zhengyin Qian fusion->fake_mag_decimation = 0.f; 55359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 55459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 55559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 unityA = *a; 55659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3ScalarMul(&unityA, l_inv); 55759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 558a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian float d = fabsf(l - NOMINAL_GRAVITY); 559a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian float p; 560a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian if (fusion->flags & FUSION_USE_GYRO) { 561a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian float fc = 0; 562a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian // Enable faster convergence 563a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian if (ACC_TRUSTWORTHY(d)) { 564a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian struct Vec3 aa; 565a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian mat33Apply(&aa, &R, &fusion->Ba); 566a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian float cos_err = vec3Dot(&aa, &unityA); 567a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian cos_err = cos_err < (1.f - ACC_COS_CONV_FACTOR) ? 568a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian (1.f - ACC_COS_CONV_FACTOR) : cos_err; 569a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian fc = (1.f - cos_err) * 570a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian (1.0f / ACC_COS_CONV_FACTOR * ACC_COS_CONV_LIMIT); 571a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian } 572a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian p = fusion->param.acc_stdev * expf(3 * d - fc); 573a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian } else { 574a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian // Adaptive acc weighting (trust acc less as it deviates from nominal g 575a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian // more), acc_stdev *= e(sqrt(| |acc| - g_nominal|)) 576a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian // 577a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian // The weighting equation comes from heuristics. 578a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian p = fusion->param.acc_stdev * expf(sqrtf(d)); 579a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian } 58059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 58159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusionUpdate(fusion, &unityA, &fusion->Ba, p); 58259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 58359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian return 0; 58459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 58559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 5866ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu#define MAG_COS_CONV_FACTOR 0.02f 5876ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu#define MAG_COS_CONV_LIMIT 3.5f 5886ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu#define MAG_STDEV_REDUCTION 0.005f // lower stdev means more trust 589a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian 5906ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xuint fusionHandleMag(struct Fusion *fusion, const struct Vec3 *m, float dT) { 59159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (!fusion_init_complete(fusion, MAG, m, 0.0f /* dT */)) { 59259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian return -EINVAL; 59359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 59459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 59559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian float magFieldSq = vec3NormSquared(m); 59659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 597a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian if (magFieldSq > MAX_VALID_MAGNETIC_FIELD_SQ 598a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian || magFieldSq < MIN_VALID_MAGNETIC_FIELD_SQ) { 5996ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusionSetMagTrust(fusion, NORMAL); 6006ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusion->lastMagInvalid = true; 60159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian return -EINVAL; 60259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 60359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 60459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Mat33 R; 60559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian fusionGetRotationMatrix(fusion, &R); 60659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 60759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 up; 60859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian mat33Apply(&up, &R, &fusion->Ba); 60959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 61059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 east; 61159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3Cross(&east, m, &up); 61259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 61359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian if (vec3NormSquared(&east) < MIN_VALID_CROSS_PRODUCT_MAG_SQ) { 6146ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusionSetMagTrust(fusion, NORMAL); 6156ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusion->lastMagInvalid = true; 61659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian return -EINVAL; 61759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian } 61859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 6196ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu if (fusion->lastMagInvalid) { 6206ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusion->lastMagInvalid = false; 6216ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusionSetMagTrust(fusion, BACK_TO_VALID); 6226ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu } 6236ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu 62459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian struct Vec3 north; 62559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3Cross(&north, &up, &east); 62659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 62759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian float invNorm = 1.0f / vec3Norm(&north); 62859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian vec3ScalarMul(&north, invNorm); 62959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 630a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian float p = fusion->param.mag_stdev; 631a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian 632a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian if (fusion->flags & FUSION_USE_GYRO) { 633a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian struct Vec3 mm; 634a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian mat33Apply(&mm, &R, &fusion->Bm); 635a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian float cos_err = vec3Dot(&mm, &north); 636a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian 6376ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu if (fusion->trustedMagDuration > 0) { 6386ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu // if the trust mag time period is not finished 6396ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu if (cos_err < (1.f - MAG_COS_CONV_FACTOR/4)) { 6406ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu // if the mag direction and the fusion north has not converged, lower the 6416ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu // standard deviation of mag to speed up convergence. 6426ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu p *= MAG_STDEV_REDUCTION; 6436ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusion->trustedMagDuration -= dT; 6446ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu } else { 6456ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu // it has converged already, so no need to keep the trust period any longer 6466ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusionSetMagTrust(fusion, NORMAL); 6476ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu } 6486ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu } else { 6496ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu cos_err = cos_err < (1.f - MAG_COS_CONV_FACTOR) ? 6506ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu (1.f - MAG_COS_CONV_FACTOR) : cos_err; 6516ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu 6526ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu float fc; 6536ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fc = (1.f - cos_err) * (1.0f / MAG_COS_CONV_FACTOR * MAG_COS_CONV_LIMIT); 6546ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu p *= expf(-fc); 6556ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu } 656a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian } 657a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian 658a7418ababbeb6645f346cb7dd756eef882680aceZhengyin Qian fusionUpdate(fusion, &north, &fusion->Bm, p); 65959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 66059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian return 0; 66159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 66259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 6636ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xuvoid fusionSetMagTrust(struct Fusion *fusion, int mode) { 6646ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu switch(mode) { 6656ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu case NORMAL: 6666ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusion->trustedMagDuration = 0; // disable 6676ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu break; 6686ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu case INITIALIZATION: 6696ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu case BACK_TO_VALID: 6706ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusion->trustedMagDuration = 0; // no special treatment for these two 6716ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu break; 6726ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu case MANUAL_MAG_CAL: 6736ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusion->trustedMagDuration = TRUST_DURATION_MANUAL_MAG_CAL; 6746ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu break; 6756ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu default: 6766ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu fusion->trustedMagDuration = 0; // by default it is disable 6776ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu break; 6786ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu } 6796ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu} 6806ecc0b705d9a05cd0ce29f8d02ade126689db388Peng Xu 68159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qianvoid fusionGetAttitude(const struct Fusion *fusion, struct Vec4 *attitude) { 68259e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian *attitude = fusion->x0; 68359e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 68459e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 68559e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qianvoid fusionGetBias(const struct Fusion *fusion, struct Vec3 *bias) { 68659e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian *bias = fusion->x1; 68759e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 68859e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian 68959e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qianvoid fusionGetRotationMatrix(const struct Fusion *fusion, struct Mat33 *R) { 69059e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian quatToMatrix(R, &fusion->x0); 69159e378eab3da179121af4c33644e40dfbad816d7Zhengyin Qian} 692