Skip to content

Commit

Permalink
fix some error such as rsqrt and add some maxtrix func
Browse files Browse the repository at this point in the history
suhetao committed Feb 15, 2015
1 parent f4c9bdc commit e0000d9
Showing 12 changed files with 404 additions and 491 deletions.
30 changes: 15 additions & 15 deletions Algorithm/src/CKF.C
Original file line number Diff line number Diff line change
@@ -26,16 +26,16 @@ CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

//////////////////////////////////////////////////////////////////////////
//all parameters below need to be tune including the weight
#define CKF_PQ_INITIAL 0.000001
#define CKF_PW_INITIAL 0.000001
#define CKF_PQ_INITIAL 0.00001
#define CKF_PW_INITIAL 0.00001

#define CKF_QQ_INITIAL 0.000045
#define CKF_QW_INITIAL 0.00174532925199432957692369076849f
#define CKF_QW_INITIAL 0.000025

#define CKF_RQ_INITIAL 0.000001
#define CKF_RA_INITIAL 0.48828125
#define CKF_RW_INITIAL 0.00174532925199432957692369076849f
#define CKF_RM_INITIAL 0.105
#define CKF_RQ_INITIAL 0.0001
#define CKF_RA_INITIAL 0.0005
#define CKF_RW_INITIAL 0.000525
#define CKF_RM_INITIAL 0.000105
//////////////////////////////////////////////////////////////////////////
//
void CKF_New(CKF_Filter* ukf)
@@ -132,16 +132,16 @@ void CKF_Init(CKF_Filter* ukf, float32_t *q, float32_t *gyro)
X[2] = q[2];
X[3] = q[3];

norm = FastInvSqrt(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
X[3] *= norm;

//initialise gyro state
X[4] = gyro[0];
X[5] = gyro[0];
X[6] = gyro[0];
X[5] = gyro[1];
X[6] = gyro[2];
}

void CKF_Update(CKF_Filter* ukf, float32_t *q, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt)
@@ -195,7 +195,7 @@ void CKF_Update(CKF_Filter* ukf, float32_t *q, float32_t *gyro, float32_t *accel
tmpX[3] = tmpQ[3] - (halfdx * tmpQ[2] - halfdy * tmpQ[1] + halfdz * tmpQ[0]);
//////////////////////////////////////////////////////////////////////////
//re-normalize quaternion
norm = FastInvSqrt(tmpX[0] * tmpX[0] + tmpX[1] * tmpX[1] + tmpX[2] * tmpX[2] + tmpX[3] * tmpX[3]);
norm = FastSqrtI(tmpX[0] * tmpX[0] + tmpX[1] * tmpX[1] + tmpX[2] * tmpX[2] + tmpX[3] * tmpX[3]);
tmpX[0] *= norm;
tmpX[1] *= norm;
tmpX[2] *= norm;
@@ -231,7 +231,7 @@ void CKF_Update(CKF_Filter* ukf, float32_t *q, float32_t *gyro, float32_t *accel
tmpX[3] = tmpQ[3] - (halfdx * tmpQ[2] - halfdy * tmpQ[1] + halfdz * tmpQ[0]);
//////////////////////////////////////////////////////////////////////////
//re-normalize quaternion
norm = FastInvSqrt(tmpX[0] * tmpX[0] + tmpX[1] * tmpX[1] + tmpX[2] * tmpX[2] + tmpX[3] * tmpX[3]);
norm = FastSqrtI(tmpX[0] * tmpX[0] + tmpX[1] * tmpX[1] + tmpX[2] * tmpX[2] + tmpX[3] * tmpX[3]);
tmpX[0] *= norm;
tmpX[1] *= norm;
tmpX[2] *= norm;
@@ -273,12 +273,12 @@ void CKF_Update(CKF_Filter* ukf, float32_t *q, float32_t *gyro, float32_t *accel
arm_mat_setcolumn_f32(&ukf->XCP, tmpX, 0);

//normalize accel and mag
norm = FastInvSqrt(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
norm = FastSqrtI(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
accel[0] *= norm;
accel[1] *= norm;
accel[2] *= norm;
//////////////////////////////////////////////////////////////////////////
norm = FastInvSqrt(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
norm = FastSqrtI(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
mag[0] *= norm;
mag[1] *= norm;
mag[2] *= norm;
@@ -440,7 +440,7 @@ void CKF_Update(CKF_Filter* ukf, float32_t *q, float32_t *gyro, float32_t *accel
arm_mat_mult_f32(&ukf->K, &ukf->Y, &ukf->tmpX);
arm_mat_add_f32(&ukf->X, &ukf->tmpX, &ukf->X);
//normalize quaternion
norm = FastInvSqrt(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
15 changes: 7 additions & 8 deletions Algorithm/src/EKF.c
Original file line number Diff line number Diff line change
@@ -106,15 +106,15 @@ void EKF_Init(EKF_Filter* ekf, float32_t *q, float32_t *gyro)
X[2] = q[2];
X[3] = q[3];

norm = FastInvSqrt(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
X[3] *= norm;

X[4] = gyro[0];
X[5] = gyro[0];
X[6] = gyro[0];
X[5] = gyro[1];
X[6] = gyro[2];
}

void EFK_Update(EKF_Filter* ekf, float32_t *q, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt)
@@ -201,7 +201,7 @@ void EFK_Update(EKF_Filter* ekf, float32_t *q, float32_t *gyro, float32_t *accel
X[3] = q3 + (halfdx * q2 - halfdy * q1 + halfdz * q0);
//////////////////////////////////////////////////////////////////////////
//Re-normalize Quaternion
norm = FastInvSqrt(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
@@ -216,14 +216,13 @@ void EFK_Update(EKF_Filter* ekf, float32_t *q, float32_t *gyro, float32_t *accel

//////////////////////////////////////////////////////////////////////////
//model and measurement differences

//normalize acc and mag measurements
norm = FastInvSqrt(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
norm = FastSqrtI(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
accel[0] *= norm;
accel[1] *= norm;
accel[2] *= norm;
//////////////////////////////////////////////////////////////////////////
norm = FastInvSqrt(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
norm = FastSqrtI(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
mag[0] *= norm;
mag[1] *= norm;
mag[2] *= norm;
@@ -343,7 +342,7 @@ void EFK_Update(EKF_Filter* ekf, float32_t *q, float32_t *gyro, float32_t *accel
arm_mat_mult_f32(&ekf->K, &ekf->Y, &ekf->tmpX);
arm_mat_add_f32(&ekf->X, &ekf->tmpX, &ekf->X);
//normalize quaternion
norm = FastInvSqrt(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
28 changes: 14 additions & 14 deletions Algorithm/src/UKF.c
Original file line number Diff line number Diff line change
@@ -26,16 +26,16 @@ CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

//////////////////////////////////////////////////////////////////////////
//all parameters below need to be tune
#define UKF_PQ_INITIAL 0.000001
#define UKF_PW_INITIAL 0.000001
#define UKF_PQ_INITIAL 0.00001
#define UKF_PW_INITIAL 0.00001

#define UKF_QQ_INITIAL 0.000045
#define UKF_QW_INITIAL 0.000025

#define UKF_RQ_INITIAL 0.000001
#define UKF_RA_INITIAL 0.07
#define UKF_RW_INITIAL 0.0525
#define UKF_RM_INITIAL 0.105
#define UKF_RQ_INITIAL 0.0001
#define UKF_RA_INITIAL 0.0005
#define UKF_RW_INITIAL 0.000525
#define UKF_RM_INITIAL 0.000105

#define UKF_alpha (1.0f)
#define UKF_beta (2.0f)
@@ -148,15 +148,15 @@ void UKF_Init(UKF_Filter* ukf, float32_t *q, float32_t *gyro)
X[2] = q[2];
X[3] = q[3];

norm = FastInvSqrt(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
X[3] *= norm;

X[4] = gyro[0];
X[5] = gyro[0];
X[6] = gyro[0];
X[5] = gyro[1];
X[6] = gyro[2];
}

void UKF_Update(UKF_Filter* ukf, float32_t *q, float32_t *gyro, float32_t *accel, float32_t *mag, float32_t dt)
@@ -201,7 +201,7 @@ void UKF_Update(UKF_Filter* ukf, float32_t *q, float32_t *gyro, float32_t *accel
tmpX[3] = tmpQ[3] - (halfdx * tmpQ[2] - halfdy * tmpQ[1] + halfdz * tmpQ[0]);
//////////////////////////////////////////////////////////////////////////
//Re-normalize Quaternion
norm = FastInvSqrt(tmpX[0] * tmpX[0] + tmpX[1] * tmpX[1] + tmpX[2] * tmpX[2] + tmpX[3] * tmpX[3]);
norm = FastSqrtI(tmpX[0] * tmpX[0] + tmpX[1] * tmpX[1] + tmpX[2] * tmpX[2] + tmpX[3] * tmpX[3]);
tmpX[0] *= norm;
tmpX[1] *= norm;
tmpX[2] *= norm;
@@ -233,7 +233,7 @@ void UKF_Update(UKF_Filter* ukf, float32_t *q, float32_t *gyro, float32_t *accel
tmpX[3] = tmpQ[3] - (halfdx * tmpQ[2] - halfdy * tmpQ[1] + halfdz * tmpQ[0]);
//////////////////////////////////////////////////////////////////////////
//re-normalize quaternion
norm = FastInvSqrt(tmpX[0] * tmpX[0] + tmpX[1] * tmpX[1] + tmpX[2] * tmpX[2] + tmpX[3] * tmpX[3]);
norm = FastSqrtI(tmpX[0] * tmpX[0] + tmpX[1] * tmpX[1] + tmpX[2] * tmpX[2] + tmpX[3] * tmpX[3]);
tmpX[0] *= norm;
tmpX[1] *= norm;
tmpX[2] *= norm;
@@ -261,12 +261,12 @@ void UKF_Update(UKF_Filter* ukf, float32_t *q, float32_t *gyro, float32_t *accel
//unscented transformation of measurments
//////////////////////////////////////////////////////////////////////////
//Normalize accel and mag
norm = FastInvSqrt(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
norm = FastSqrtI(accel[0] * accel[0] + accel[1] * accel[1] + accel[2] * accel[2]);
accel[0] *= norm;
accel[1] *= norm;
accel[2] *= norm;
//////////////////////////////////////////////////////////////////////////
norm = FastInvSqrt(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
norm = FastSqrtI(mag[0] * mag[0] + mag[1] * mag[1] + mag[2] * mag[2]);
mag[0] *= norm;
mag[1] *= norm;
mag[2] *= norm;
@@ -398,7 +398,7 @@ void UKF_Update(UKF_Filter* ukf, float32_t *q, float32_t *gyro, float32_t *accel
arm_mat_mult_f32(&ukf->K, &ukf->Y, &ukf->tmpX);
arm_mat_add_f32(&ukf->X, &ukf->tmpX, &ukf->X);
//normalize quaternion
norm = FastInvSqrt(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
norm = FastSqrtI(X[0] * X[0] + X[1] * X[1] + X[2] * X[2] + X[3] * X[3]);
X[0] *= norm;
X[1] *= norm;
X[2] *= norm;
14 changes: 7 additions & 7 deletions Application/Src/main.c
Original file line number Diff line number Diff line change
@@ -34,8 +34,8 @@ CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

//uncomment one
//#define USE_6AXIS_EKF
#define USE_6AXIS_FP_EKF
//#define USE_EKF
//#define USE_6AXIS_FP_EKF
#define USE_EKF
//#define USE_UKF
//#define USE_CKF

@@ -52,8 +52,8 @@ CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#endif

static s8 gyro_orientation[9] = {
-1, 0, 0,
0,-1, 0,
1, 0, 0,
0, 1, 0,
0, 0, 1
};

@@ -195,9 +195,9 @@ int main(void)
fRealGyro[1] = GYRO_TORAD(s16Gyro[1]);
fRealGyro[2] = GYRO_TORAD(s16Gyro[2]);

fRealAccel[0] = s16Accel[0];
fRealAccel[1] = s16Accel[1];
fRealAccel[2] = s16Accel[2];
fRealAccel[0] = s16Accel[0] / 16384.0f;
fRealAccel[1] = s16Accel[1] / 16384.0f;
fRealAccel[2] = s16Accel[2] / 16384.0f;

fRealMag[0] = s16Mag[0];
fRealMag[1] = s16Mag[1];
6 changes: 3 additions & 3 deletions Matrix/src/Matrix.c
Original file line number Diff line number Diff line change
@@ -126,9 +126,9 @@ arm_status arm_mat_chol_f32(arm_matrix_instance_f32* s)
if (*p_Lkk <= 0.0f){
return ARM_MATH_SINGULAR;
}
//arm_sqrt_f32(*p_Lkk, p_Lkk);
//reciprocal = 1.0f / *p_Lkk;
reciprocal = FastInvSqrt(*p_Lkk);
arm_sqrt_f32(*p_Lkk, p_Lkk);
reciprocal = 1.0f / *p_Lkk;
//reciprocal = FastSqrtI(*p_Lkk);
p_Li0 = p_Lk0 + n;
for (i = k + 1; i < n; p_Li0 += n, i++) {
for (p = 0; p < k; p++)
Loading

0 comments on commit e0000d9

Please sign in to comment.