/*! return its inverse matrix */
inline _zgematrix i(const zgbmatrix& mat)
{
#ifdef CPPL_VERBOSE
std::cerr << "# [MARK] i(const zgbmatrix&)"
<< std::endl;
#endif//CPPL_VERBOSE
#ifdef CPPL_DEBUG
if(mat.M!=mat.N){
std::cerr << "[ERROR] i(zgbmatrix&) " << std::endl
<< "This matrix is not square and has no inverse matrix."
<< std::endl
<< "Your input was (" << mat.M << "x" << mat.N << ")."
<< std::endl;
exit(1);
}
#endif//CPPL_DEBUG
zgbmatrix mat_cp(mat);
zgematrix mat_inv(mat.M,mat.N);
mat_inv.identity();
mat_cp.zgbsv(mat_inv);
return _(mat_inv);
}
static void kalman_correct(kalman_t *kf, float pos, float speed)
{
/* update H matrix: */
kf->H.me[1][1] = 1.0f * (kf->use_speed && speed != 0.0f);
kf->z.ve[0] = pos;
kf->z.ve[1] = speed;
/* K = P * HT * inv(H * P * HT + R) */
mat_mul(&kf->T0, &kf->H, &kf->P); // T0 = H * P
mmtr_mul(&kf->T1, &kf->T0, &kf->H); // T1 = T0 * HT
mat_add(&kf->T0, &kf->T1, &kf->R); // T0 = T1 + R
mat_inv(&kf->T1, &kf->T0); // T1 = inv(T0)
mmtr_mul(&kf->T0, &kf->P, &kf->H); // T0 = P * HT
mat_mul(&kf->K, &kf->T0, &kf->T1); // K = T0 * T1
/* x = x + K * (z - H * x) */
mat_vec_mul(&kf->t0, &kf->H, &kf->x); // t0 = H * x
vec_sub(&kf->t1, &kf->z, &kf->t0); // t1 = z - t0
mat_vec_mul(&kf->t0, &kf->K, &kf->t1); // t0 = K * t1
vec_add(&kf->x, &kf->x, &kf->t0); // x = x + t0
/* P = (I - K * H) * P */
mat_mul(&kf->T0, &kf->K, &kf->H); // T0 = K * H
mat_sub(&kf->T1, &kf->I, &kf->T0); // T1 = I - T0
mat_mul(&kf->T0, &kf->T1, &kf->P); // T0 = T1 * P
mat_copy(&kf->P, &kf->T0); // P = T0
}
/*! return its inverse matrix */
inline _zgematrix i(const zhematrix& mat)
{VERBOSE_REPORT;
zhematrix mat_cp(mat);
zgematrix mat_inv(mat.n,mat.n);
mat_inv.identity();
mat_cp.zhesv(mat_inv);
return _(mat_inv);
}
static int init_tilat()
{
int jj;
int i,j,gr;
// double det;
/* sur_print("\nInitializing work matrices..."); */
hav=muste_fopen(tempfile,"r+b");
for (i=0; i<ng; ++i)
{
N1[i]=0.0;
for (j=0; j<m; ++j) S[i+ng*j]=0.0;
}
for (jj=0L; jj<n; ++jj)
{
hav_read1(jj,&gr); --gr;
hav_read3(jj,xx);
++N1[gr];
for (i=0; i<m; ++i) S[gr+ng*i]+=xx[i];
}
for (i=0; i<ng; ++i)
{
if (N1[i]==0.0)
{
if (maxiter>1) { muste_fclose(hav); return(-1); }
sprintf(sbuf,"\nNo cases in initial setting for group %d",i+1);
sur_print(sbuf); WAIT; return(-1);
}
}
mat_mmt(H1,S,ng,m);
for (i=0; i<ng; ++i)
for (j=0; j<ng; ++j)
{
if (i==j) H1[i+ng*j]=N1[i]-H1[i+ng*j];
else H1[i+ng*j]=-H1[i+ng*j];
}
mat_inv(H2,H1,ng,&f2);
for (i=0; i<ng; ++i) f2/=N1[i];
/* for (i=0; i<ng2*ng2; ++i) Q[i]=0.0; tarpeeton */
for (i=0; i<ng; ++i)
for (j=0; j<ng; ++j)
Q[i+ng2*j]=H2[i+ng*j];
/* sprintf(sbuf,"\nlambda=%g",f2); sur_print(sbuf); */
/* Rprintf("\nH2:"); matprint(H2,ng,ng); */
muste_fclose(hav);
return(1);
}
double Gaussian_3d::pdf_3d(Sample_3d x){
/* determinante mat covarianza */
double det = det_3d(cov);
//std::cout<<"Determinante: "<<det<<std::endl;
/* vettore X - Mu */
double diff[3];
diff[0] = x[0] - mean[0];
diff[1] = x[1] - mean[1];
diff[2] = x[2] - mean[2];
/* calcolo matrice inversa delle covarianze */
boost::numeric::ublas::matrix<double> mat_inv(3,3);
inv_3d(cov, mat_inv);
//std::cout<<"inversa delle covarianze: "<<mat_inv<<std::endl;
/* calcolo esponente della funzione gaussiana: (X - Mu) * Cov^(-1) * (X - Mu)' */
double mahalanobis_dis =
(diff[0] * mat_inv(0,0) + diff[1] * mat_inv(1,0) + diff[2] * mat_inv(2,0)) * diff[0] +
(diff[0] * mat_inv(0,1) + diff[1] * mat_inv(1,1) + diff[2] * mat_inv(2,1)) * diff[1] +
(diff[0] * mat_inv(0,2) + diff[1] * mat_inv(1,2) + diff[2] * mat_inv(2,2)) * diff[2];
//std::cout<<"esponente della gaussiana: "<<mahalanobis_dis<<std::endl;
/* calcolo coefficiente gaussiana */
double pre = pow((2 * M_PI), 1.5) * pow(det, 0.5);
/* calcolo esponenziale */
double exp = pow(M_E, -0.5 * mahalanobis_dis);
//std::cout<<"esponenziale gaussiana: "<<exp<<std::endl;
//std::cout<<"coefficiente gaussiana: "<<pre<<std::endl;
return pre/exp;
}
/*! return its inverse matrix */
inline _zgematrix i(const _zhematrix& mat)
{
#ifdef CPPL_VERBOSE
std::cerr << "# [MARK] i(const _zhematrix&)"
<< std::endl;
#endif//CPPL_VERBOSE
zhematrix mat_cp;
mat_cp.shallow_copy(mat);
zgematrix mat_inv(mat.N,mat.N);
mat_inv.identity();
mat_cp.zhesv(mat_inv);
return _(mat_inv);
}
/*! return its inverse matrix */
inline _zgematrix i(const _zgematrix& mat)
{VERBOSE_REPORT;
#ifdef CPPL_DEBUG
if(mat.m!=mat.n){
ERROR_REPORT;
std::cerr << "This matrix is not square and has no inverse matrix." << std::endl
<< "Your input was (" << mat.m << "x" << mat.n << ")." << std::endl;
exit(1);
}
#endif//CPPL_DEBUG
zgematrix mat_cp(mat);
zgematrix mat_inv(mat_cp.m,mat_cp.n);
mat_inv.identity();
mat_cp.zgesv(mat_inv);
return _(mat_inv);
}
//---------------------------------------------------------------------------
// Compute inverse matrix
NaMatrix& NaMatrix::inv (NaMatrix& mInv) const
{
#ifdef __matrix_h
if(dim_rows() != dim_cols()){
throw(na_size_mismatch);
}
MATRIX mat = mat_creat(dim_rows(), dim_cols(), UNDEFINED);
unsigned i, j, n = dim_rows();
for(i = 0; i < n; ++i){
for(j = 0; j < n; ++j){
mat[i][j] = get(i, j);
}
}
MATRIX minv = mat_inv(mat);
if(NULL == minv){
throw(na_bad_value);
}
mat_free(mat);
mInv.new_dim(n, n);
for(i = 0; i < n; ++i){
for(j = 0; j < n; ++j){
mInv.fetch(i, j) = minv[i][j];
}
}
mat_free(minv);
#if 0
trans(mInv);
NaReal vDet = det();
if(0 == vDet)
throw(na_bad_value);
else
mInv.multiply(1/vDet);
#endif
return mInv;
#else
throw(na_not_implemented);
#endif /* __matrix_h */
}
static double T2_BF()
{
int i,j;
double t2,det;
if (orig_samples)
{
laske_summat(ss[0],ss2[0]);
for (i=0; i<m; ++i)
{
ss[1][i]=s[i]-ss[0][i];
for (j=0; j<=i; ++j) ss2[1][i+m*j]=s2[i+m*j]-ss2[0][i+m*j];
}
for (i=0; i<m; ++i)
for (j=0; j<=i; ++j)
{
ss_apu[i+m*j]=
(ss2[0][i+m*j]-ss[0][i]*ss[0][j]/n[1])/(n[1]*(n[1]-1))
+(ss2[1][i+m*j]-ss[1][i]*ss[1][j]/n[2])/(n[2]*(n[2]-1));
ss_apu[j+m*i]=ss_apu[i+m*j];
}
mat_inv(ss_inv,ss_apu,m,&det);
// mprint(ss_inv,m,m);
}
else
laske_summat(ss[0],NULL);
for (i=0; i<m; ++i)
{
ss[0][i]/=n[1];
ss[1][i]/=n[2];
}
for (i=0; i<m; ++i) ss_apu[i]=ss[0][i]-ss[1][i];
t2=0.0;
for (i=0; i<m; ++i)
{
t2+=ss_apu[i]*ss_apu[i]*ss_inv[i*(m+1)];
for (j=0; j<i; ++j)
t2+=2*ss_apu[i]*ss_apu[j]*ss_inv[i+m*j];
}
return (t2);
} /* T2_BF */
/*----------------------------------------------------------------------------*/
static MATRIX buildRMatrix(MATRIX UB, MATRIX planeNormal,
tasQEPosition qe, int *errorCode){
MATRIX U1V, U2V, TV, TVINV, M;
*errorCode = 1;
U1V = tasReflectionToQC(qe,UB);
if(U1V == NULL){
*errorCode = UBNOMEMORY;
return NULL;
}
normalizeVector(U1V);
U2V = vectorCrossProduct(planeNormal,U1V);
if(U2V == NULL){
killVector(U1V);
*errorCode = UBNOMEMORY;
return NULL;
}
if(vectorLength(U2V) < .0001){
*errorCode = BADUBORQ;
killVector(U1V);
killVector(U2V);
return NULL;
}
TV = buildTVMatrix(U1V,U2V);
if(TV == NULL){
killVector(U1V);
killVector(U2V);
*errorCode = UBNOMEMORY;
return NULL;
}
TVINV = mat_inv(TV);
if(TVINV == NULL){
*errorCode = BADUBORQ;
}
killVector(U1V);
killVector(U2V);
mat_free(TV);
return TVINV;
}
/*------------------------------------------------------------------------*/
int calcTasQH(MATRIX UB, tasAngles angles, ptasQEPosition qe){
MATRIX UBINV = NULL, QV = NULL, Q = NULL;
double q;
tasReflection r;
int i;
UBINV = mat_inv(UB);
r.angles = angles;
r.qe = *qe;
QV = calcTasUVectorFromAngles(r);
if(UBINV == NULL || QV == NULL){
return UBNOMEMORY;
}
/*
normalize the QV vector to be the length of the Q vector
Thereby take into account the physicists magic fudge
2PI factor
*/
q = sqrt(qe->ki*qe->ki + qe->kf*qe->kf -
2.*qe->ki*qe->kf*Cosd(angles.sample_two_theta));
qe->qm = q;
q /= 2. * PI;
for(i = 0; i < 3; i++){
QV[i][0] *= q;
}
Q = mat_mul(UBINV,QV);
if(Q == NULL){
mat_free(UBINV);
killVector(QV);
return UBNOMEMORY;
}
qe->qh = Q[0][0];
qe->qk = Q[1][0];
qe->ql = Q[2][0];
killVector(QV);
killVector(Q);
mat_free(UBINV);
return 1;
}
//--------------------------------------------------------------------------
// void
// DiracOpClover::CloverMatChkb(ChkbType chkb, int inv) const
//--------------------------------------------------------------------------
// Purpose:
// Calculates the clover matrix for all sites of the specified checkerboard
// and stores them in the memory pointed by lat.Aux0Ptr() for
// chkb CHKB_EVEN and by lat.Aux1Ptr() for chkb CHKB_ODD.
// Arguments:
// site[0..3] coordinate[x,y,z,t]
// Convension:
// Links and fermionic fields are in Wilson order, that is
// checkerboarded U[t][y][z][x][x,y,z,t].
//--------------------------------------------------------------------------
void
DiracOpClover::CloverMatChkb(ChkbType chkb, int inv) const
{
char *fname = "CloverMatChkb()";
VRB.Func(cname,fname);
// get the pointer to the clover matrices.
//------------------------------------------------------------------------
IFloat *cl_mat_p = (IFloat *)(chkb == CHKB_EVEN ?
lat.Aux0Ptr() : lat.Aux1Ptr());
// loop in the order consistent with the Wilson checkerboarded storage
// Note: the sites[0,1,2,3] is in order of [x,y,z,t]
//------------------------------------------------------------------------
{
IFloat *Ap = cl_mat_p;
const int parity = (chkb == CHKB_ODD ? 1 : 0);
const int *nsites = clover_lib_arg->nsites;
int site[4];
for (site[3] = 0; site[3] < nsites[3]; ++(site[3])) {
for (site[2] = 0; site[2] < nsites[2]; ++(site[2])) {
for (site[1] = 0; site[1] < nsites[1]; ++(site[1])) {
site[0] = (site[3] + site[2] + site[1] + parity)%2;
for (; site[0] < nsites[0]; site[0] += 2) {
SiteCloverMat(site, Ap);
Ap += CLOVER_MAT_SIZE;
}
}
}
}
}
// invert the clover matrix
//------------------------------------------------------------------------
if (inv) {
IFloat *Ap = cl_mat_p;
for (int i = 0; i < GJP.VolNodeSites(); ++i) {
mat_inv(Ap, Ap, 6, MAT_INV_ALG_LDL_CMPR, 0);
Ap += HALF_CLOVER_MAT_SIZE;
}
}
}
static void remove_system_drift(struct md *md)
{
vec_t cp = get_system_com(md);
vec_t cv = get_system_com_velocity(md);
vec_t am = get_system_angular_momentum(md);
mat_t inertia = get_system_inertia_tensor(md);
mat_t inertia_inv = mat_zero;
if (inertia.xx < EPSILON ||
inertia.yy < EPSILON ||
inertia.zz < EPSILON) {
inertia_inv.xx = inertia.xx < EPSILON ? 0.0 : 1.0 / inertia.xx;
inertia_inv.yy = inertia.yy < EPSILON ? 0.0 : 1.0 / inertia.yy;
inertia_inv.zz = inertia.zz < EPSILON ? 0.0 : 1.0 / inertia.zz;
}
else {
inertia_inv = mat_inv(&inertia);
}
vec_t av = mat_vec(&inertia_inv, &am);
for (size_t i = 0; i < md->n_bodies; i++) {
struct body *body = md->bodies + i;
vec_t pos = wrap(md, &body->pos);
vec_t cross = {
av.y * (pos.z - cp.z) - av.z * (pos.y - cp.y),
av.z * (pos.x - cp.x) - av.x * (pos.z - cp.z),
av.x * (pos.y - cp.y) - av.y * (pos.x - cp.x)
};
body->vel.x -= cv.x + cross.x;
body->vel.y -= cv.y + cross.y;
body->vel.z -= cv.z + cross.z;
}
vec_t cv2 = get_system_com_velocity(md);
vec_t am2 = get_system_angular_momentum(md);
assert(vec_len(&cv2) < EPSILON && vec_len(&am2) < EPSILON);
}
/*! return its inverse matrix */
inline _dsymatrix i(const _dsymatrix& mat)
{VERBOSE_REPORT;
dsymatrix mat_cp(mat);
dsymatrix mat_inv(mat_cp.n);
mat_inv.identity();
char UPLO('l');
long NRHS(mat.n), LDA(mat.n), *IPIV(new long[mat.n]), LDB(mat.n), LWORK(-1), INFO(1);
double *WORK( new double[1] );
dsysv_(UPLO, mat_cp.n, NRHS, mat_cp.array, LDA, IPIV, mat_inv.array, LDB, WORK, LWORK, INFO);
LWORK = long(WORK[0]);
delete [] WORK; WORK = new double[LWORK];
dsysv_(UPLO, mat_cp.n, NRHS, mat_cp.array, LDA, IPIV, mat_inv.array, LDB, WORK, LWORK, INFO);
delete [] WORK; delete [] IPIV;
if(INFO!=0){
WARNING_REPORT;
std::cerr << "Serious trouble happend. INFO = " << INFO << "." << std::endl;
}
return _(mat_inv);
}
static double T2()
{
int i,j;
double t2,det;
if (orig_samples)
{
laske_summat(ss[0],ss2[0]);
for (i=0; i<m; ++i)
{
ss[1][i]=s[i]-ss[0][i];
for (j=0; j<=i; ++j) ss2[1][i+m*j]=s2[i+m*j]-ss2[0][i+m*j];
}
}
else
laske_summat(ss[0],NULL);
for (i=0; i<m; ++i)
{
ss[0][i]/=n[1];
ss[1][i]/=n[2];
}
// Rprintf("ka:\n");
// mprint(ss[0],1,m);
// mprint(ss[1],1,m);
if (orig_samples)
{
for (i=0; i<m; ++i)
for (j=0; j<=i; ++j)
{
ss2[0][i+m*j]+=-n[1]*ss[0][i]*ss[0][j]
+ss2[1][i+m*j]-n[2]*ss[1][i]*ss[1][j];
ss2[0][j+m*i]=ss2[0][i+m*j];
}
for (i=0; i<m; ++i)
for (j=0; j<=i; ++j)
{
ss2[0][i+m*j]/=n[0]-2;
ss2[0][j+m*i]=ss2[0][i+m*j];
}
// mprint(ss2[0],m,m);
mat_inv(ss2[1],ss2[0],m,&det);
// mat_inv() -> mat_gj() joka varaa 3 vektoritilaa, mutta vapauttaa ne
// mprint(ss2[1],m,m);
}
for (i=0; i<m; ++i) ss[0][i]-=ss[1][i];
t2=0.0;
for (i=0; i<m; ++i)
{
t2+=ss[0][i]*ss[0][i]*ss2[1][i*(m+1)];
for (j=0; j<i; ++j)
t2+=2*ss[0][i]*ss[0][j]*ss2[1][i+m*j];
}
t2*=(double)n[1]*(double)n[2]/(double)n[0];
return (t2);
}
int
main(void)
{
int i, result;
flint_rand_t state;
printf("inv... ");
fflush(stdout);
_randinit(state);
/* Check aliasing */
/* Managed element type (mpq_t) */
for (i = 0; i < 50; i++)
{
long n;
ctx_t ctx;
mat_t A, B, C;
long ansB, ansC;
n = n_randint(state, 20) + 1;
ctx_init_mpq(ctx);
mat_init(A, n, n, ctx);
mat_init(B, n, n, ctx);
mat_init(C, n, n, ctx);
mat_randtest(A, state, ctx);
mat_set(B, A, ctx);
ansC = mat_inv(C, B, ctx);
ansB = mat_inv(B, B, ctx);
result = ((ansB == ansC) && (!ansB || mat_equal(B, C, ctx)));
if (!result)
{
printf("FAIL:\n\n");
printf("A: \n"), mat_print(A, ctx), printf("\n");
printf("B: \n"), mat_print(B, ctx), printf("\n");
printf("C: \n"), mat_print(C, ctx), printf("\n");
printf("ansB = %ld\n", ansB);
printf("ansC = %ld\n", ansC);
}
mat_clear(A, ctx);
mat_clear(B, ctx);
mat_clear(C, ctx);
ctx_clear(ctx);
}
/* Check A * A^{-1} == A^{-1} * A == Id */
/* Managed element type (mpq_t) */
for (i = 0; i < 50; i++)
{
long n;
ctx_t ctx;
mat_t A, B, C, D, I;
long ansB;
n = n_randint(state, 20) + 1;
ctx_init_mpq(ctx);
mat_init(A, n, n, ctx);
mat_init(B, n, n, ctx);
mat_init(C, n, n, ctx);
mat_init(D, n, n, ctx);
mat_init(I, n, n, ctx);
mat_randtest(A, state, ctx);
ansB = mat_inv(B, A, ctx);
if (!ansB)
{
mat_mul(C, A, B, ctx);
mat_mul(D, B, A, ctx);
}
result = (ansB || (mat_equal(C, D, ctx) && mat_is_one(C, ctx)));
if (!result)
{
printf("FAIL:\n\n");
printf("A: \n"), mat_print(A, ctx), printf("\n");
printf("B: \n"), mat_print(B, ctx), printf("\n");
printf("C: \n"), mat_print(C, ctx), printf("\n");
printf("D: \n"), mat_print(D, ctx), printf("\n");
printf("ansB = %ld\n", ansB);
}
mat_clear(A, ctx);
mat_clear(B, ctx);
mat_clear(C, ctx);
mat_clear(D, ctx);
ctx_clear(ctx);
}
_randclear(state);
_fmpz_cleanup();
printf("PASS\n");
return EXIT_SUCCESS;
}
// Main get_nav filter function
void get_nav(struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){
double tnow, imu_dt;
double dq[4], quat_new[4];
// compute time-elapsed 'dt'
// This compute the navigation state at the DAQ's Time Stamp
tnow = sensorData_ptr->imuData_ptr->time;
imu_dt = tnow - tprev;
tprev = tnow;
// ================== Time Update ===================
// Temporary storage in Matrix form
quat[0] = navData_ptr->quat[0];
quat[1] = navData_ptr->quat[1];
quat[2] = navData_ptr->quat[2];
quat[3] = navData_ptr->quat[3];
a_temp31[0][0] = navData_ptr->vn; a_temp31[1][0] = navData_ptr->ve;
a_temp31[2][0] = navData_ptr->vd;
b_temp31[0][0] = navData_ptr->lat; b_temp31[1][0] = navData_ptr->lon;
b_temp31[2][0] = navData_ptr->alt;
// AHRS Transformations
C_N2B = quat2dcm(quat, C_N2B);
C_B2N = mat_tran(C_N2B, C_B2N);
// Attitude Update
// ... Calculate Navigation Rate
nr = navrate(a_temp31,b_temp31,nr);
dq[0] = 1;
dq[1] = 0.5*om_ib[0][0]*imu_dt;
dq[2] = 0.5*om_ib[1][0]*imu_dt;
dq[3] = 0.5*om_ib[2][0]*imu_dt;
qmult(quat,dq,quat_new);
quat[0] = quat_new[0]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[1] = quat_new[1]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[2] = quat_new[2]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[3] = quat_new[3]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
if(quat[0] < 0) {
// Avoid quaternion flips sign
quat[0] = -quat[0];
quat[1] = -quat[1];
quat[2] = -quat[2];
quat[3] = -quat[3];
}
navData_ptr->quat[0] = quat[0];
navData_ptr->quat[1] = quat[1];
navData_ptr->quat[2] = quat[2];
navData_ptr->quat[3] = quat[3];
quat2eul(navData_ptr->quat,&(navData_ptr->phi),&(navData_ptr->the),&(navData_ptr->psi));
// Velocity Update
dx = mat_mul(C_B2N,f_b,dx);
dx = mat_add(dx,grav,dx);
navData_ptr->vn += imu_dt*dx[0][0];
navData_ptr->ve += imu_dt*dx[1][0];
navData_ptr->vd += imu_dt*dx[2][0];
// Position Update
dx = llarate(a_temp31,b_temp31,dx);
navData_ptr->lat += imu_dt*dx[0][0];
navData_ptr->lon += imu_dt*dx[1][0];
navData_ptr->alt += imu_dt*dx[2][0];
// JACOBIAN
F = mat_fill(F, ZERO_MATRIX);
// ... pos2gs
F[0][3] = 1.0; F[1][4] = 1.0; F[2][5] = 1.0;
// ... gs2pos
F[5][2] = -2*g/EARTH_RADIUS;
// ... gs2att
temp33 = sk(f_b,temp33);
atemp33 = mat_mul(C_B2N,temp33,atemp33);
F[3][6] = -2.0*atemp33[0][0]; F[3][7] = -2.0*atemp33[0][1]; F[3][8] = -2.0*atemp33[0][2];
F[4][6] = -2.0*atemp33[1][0]; F[4][7] = -2.0*atemp33[1][1]; F[4][8] = -2.0*atemp33[1][2];
F[5][6] = -2.0*atemp33[2][0]; F[5][7] = -2.0*atemp33[2][1]; F[5][8] = -2.0*atemp33[2][2];
// ... gs2acc
F[3][9] = -C_B2N[0][0]; F[3][10] = -C_B2N[0][1]; F[3][11] = -C_B2N[0][2];
F[4][9] = -C_B2N[1][0]; F[4][10] = -C_B2N[1][1]; F[4][11] = -C_B2N[1][2];
F[5][9] = -C_B2N[2][0]; F[5][10] = -C_B2N[2][1]; F[5][11] = -C_B2N[2][2];
// ... att2att
temp33 = sk(om_ib,temp33);
F[6][6] = -temp33[0][0]; F[6][7] = -temp33[0][1]; F[6][8] = -temp33[0][2];
F[7][6] = -temp33[1][0]; F[7][7] = -temp33[1][1]; F[7][8] = -temp33[1][2];
F[8][6] = -temp33[2][0]; F[8][7] = -temp33[2][1]; F[8][8] = -temp33[2][2];
// ... att2gyr
F[6][12] = -0.5;
F[7][13] = -0.5;
F[8][14] = -0.5;
// ... Accel Markov Bias
F[9][9] = -1.0/TAU_A; F[10][10] = -1.0/TAU_A; F[11][11] = -1.0/TAU_A;
F[12][12] = -1.0/TAU_G; F[13][13] = -1.0/TAU_G; F[14][14] = -1.0/TAU_G;
//fprintf(stderr,"Jacobian Created\n");
// State Transition Matrix: PHI = I15 + F*dt;
temp1515 = mat_scalMul(F,imu_dt,temp1515);
PHI = mat_add(I15,temp1515,PHI);
// Process Noise
G = mat_fill(G, ZERO_MATRIX);
G[3][0] = -C_B2N[0][0]; G[3][1] = -C_B2N[0][1]; G[3][2] = -C_B2N[0][2];
G[4][0] = -C_B2N[1][0]; G[4][1] = -C_B2N[1][1]; G[4][2] = -C_B2N[1][2];
G[5][0] = -C_B2N[2][0]; G[5][1] = -C_B2N[2][1]; G[5][2] = -C_B2N[2][2];
G[6][3] = -0.5;
G[7][4] = -0.5;
G[8][5] = -0.5;
G[9][6] = 1.0; G[10][7] = 1.0; G[11][8] = 1.0;
G[12][9] = 1.0; G[13][10] = 1.0; G[14][11] = 1.0;
//fprintf(stderr,"Process Noise Matrix G is created\n");
// Discrete Process Noise
temp1512 = mat_mul(G,Rw,temp1512);
temp1515 = mat_transmul(temp1512,G,temp1515); // Qw = G*Rw*G'
Qw = mat_scalMul(temp1515,imu_dt,Qw); // Qw = dt*G*Rw*G'
Q = mat_mul(PHI,Qw,Q); // Q = (I+F*dt)*Qw
temp1515 = mat_tran(Q,temp1515);
Q = mat_add(Q,temp1515,Q);
Q = mat_scalMul(Q,0.5,Q); // Q = 0.5*(Q+Q')
//fprintf(stderr,"Discrete Process Noise is created\n");
// Covariance Time Update
temp1515 = mat_mul(PHI,P,temp1515);
P = mat_transmul(temp1515,PHI,P); // P = PHI*P*PHI'
P = mat_add(P,Q,P); // P = PHI*P*PHI' + Q
temp1515 = mat_tran(P, temp1515);
P = mat_add(P,temp1515,P);
P = mat_scalMul(P,0.5,P); // P = 0.5*(P+P')
//fprintf(stderr,"Covariance Updated through Time Update\n");
navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2];
navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5];
navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8];
navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11];
navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14];
navData_ptr->err_type = TU_only;
//fprintf(stderr,"Time Update Done\n");
// ================== DONE TU ===================
if(sensorData_ptr->gpsData_ptr->newData){
// ================== GPS Update ===================
sensorData_ptr->gpsData_ptr->newData = 0; // Reset the flag
// Position, converted to NED
a_temp31[0][0] = navData_ptr->lat;
a_temp31[1][0] = navData_ptr->lon; a_temp31[2][0] = navData_ptr->alt;
pos_ins_ecef = lla2ecef(a_temp31,pos_ins_ecef);
a_temp31[2][0] = 0.0;
//pos_ref = lla2ecef(a_temp31,pos_ref);
pos_ref = mat_copy(a_temp31,pos_ref);
pos_ins_ned = ecef2ned(pos_ins_ecef,pos_ins_ned,pos_ref);
pos_gps[0][0] = sensorData_ptr->gpsData_ptr->lat*D2R;
pos_gps[1][0] = sensorData_ptr->gpsData_ptr->lon*D2R;
pos_gps[2][0] = sensorData_ptr->gpsData_ptr->alt;
pos_gps_ecef = lla2ecef(pos_gps,pos_gps_ecef);
pos_gps_ned = ecef2ned(pos_gps_ecef,pos_gps_ned,pos_ref);
// Create Measurement: y
y[0][0] = pos_gps_ned[0][0] - pos_ins_ned[0][0];
y[1][0] = pos_gps_ned[1][0] - pos_ins_ned[1][0];
y[2][0] = pos_gps_ned[2][0] - pos_ins_ned[2][0];
y[3][0] = sensorData_ptr->gpsData_ptr->vn - navData_ptr->vn;
y[4][0] = sensorData_ptr->gpsData_ptr->ve - navData_ptr->ve;
y[5][0] = sensorData_ptr->gpsData_ptr->vd - navData_ptr->vd;
//fprintf(stderr,"Measurement Matrix, y, created\n");
// Kalman Gain
temp615 = mat_mul(H,P,temp615);
temp66 = mat_transmul(temp615,H,temp66);
atemp66 = mat_add(temp66,R,atemp66);
temp66 = mat_inv(atemp66,temp66); // temp66 = inv(H*P*H'+R)
//fprintf(stderr,"inv(H*P*H'+R) Computed\n");
temp156 = mat_transmul(P,H,temp156); // P*H'
//fprintf(stderr,"P*H' Computed\n");
K = mat_mul(temp156,temp66,K); // K = P*H'*inv(H*P*H'+R)
//fprintf(stderr,"Kalman Gain Computed\n");
// Covariance Update
temp1515 = mat_mul(K,H,temp1515);
ImKH = mat_sub(I15,temp1515,ImKH); // ImKH = I - K*H
temp615 = mat_transmul(R,K,temp615);
KRKt = mat_mul(K,temp615,KRKt); // KRKt = K*R*K'
temp1515 = mat_transmul(P,ImKH,temp1515);
P = mat_mul(ImKH,temp1515,P); // ImKH*P*ImKH'
temp1515 = mat_add(P,KRKt,temp1515);
P = mat_copy(temp1515,P); // P = ImKH*P*ImKH' + KRKt
//fprintf(stderr,"Covariance Updated through GPS Update\n");
navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2];
navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5];
navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8];
navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11];
navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14];
// State Update
x = mat_mul(K,y,x);
denom = (1.0 - (ECC2 * sin(navData_ptr->lat) * sin(navData_ptr->lat)));
denom = sqrt(denom*denom);
Re = EARTH_RADIUS / sqrt(denom);
Rn = EARTH_RADIUS*(1-ECC2) / denom*sqrt(denom);
navData_ptr->alt = navData_ptr->alt - x[2][0];
navData_ptr->lat = navData_ptr->lat + x[0][0]/(Re + navData_ptr->alt);
navData_ptr->lon = navData_ptr->lon + x[1][0]/(Rn + navData_ptr->alt)/cos(navData_ptr->lat);
navData_ptr->vn = navData_ptr->vn + x[3][0];
navData_ptr->ve = navData_ptr->ve + x[4][0];
navData_ptr->vd = navData_ptr->vd + x[5][0];
quat[0] = navData_ptr->quat[0];
quat[1] = navData_ptr->quat[1];
quat[2] = navData_ptr->quat[2];
quat[3] = navData_ptr->quat[3];
// Attitude correction
dq[0] = 1.0;
dq[1] = x[6][0];
dq[2] = x[7][0];
dq[3] = x[8][0];
qmult(quat,dq,quat_new);
quat[0] = quat_new[0]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[1] = quat_new[1]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[2] = quat_new[2]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
quat[3] = quat_new[3]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
navData_ptr->quat[0] = quat[0];
navData_ptr->quat[1] = quat[1];
navData_ptr->quat[2] = quat[2];
navData_ptr->quat[3] = quat[3];
quat2eul(navData_ptr->quat,&(navData_ptr->phi),&(navData_ptr->the),&(navData_ptr->psi));
navData_ptr->ab[0] = navData_ptr->ab[0] + x[9][0];
navData_ptr->ab[1] = navData_ptr->ab[1] + x[10][0];
navData_ptr->ab[2] = navData_ptr->ab[2] + x[11][0];
navData_ptr->gb[0] = navData_ptr->gb[0] + x[12][0];
navData_ptr->gb[1] = navData_ptr->gb[1] + x[13][0];
navData_ptr->gb[2] = navData_ptr->gb[2] + x[14][0];
navData_ptr->err_type = gps_aided;
//fprintf(stderr,"Measurement Update Done\n");
}
// Remove current estimated biases from rate gyro and accels
sensorData_ptr->imuData_ptr->p -= navData_ptr->gb[0];
sensorData_ptr->imuData_ptr->q -= navData_ptr->gb[1];
sensorData_ptr->imuData_ptr->r -= navData_ptr->gb[2];
sensorData_ptr->imuData_ptr->ax -= navData_ptr->ab[0];
sensorData_ptr->imuData_ptr->ay -= navData_ptr->ab[1];
sensorData_ptr->imuData_ptr->az -= navData_ptr->ab[2];
// Get the new Specific forces and Rotation Rate,
// use in the next time update
f_b[0][0] = sensorData_ptr->imuData_ptr->ax;
f_b[1][0] = sensorData_ptr->imuData_ptr->ay;
f_b[2][0] = sensorData_ptr->imuData_ptr->az;
om_ib[0][0] = sensorData_ptr->imuData_ptr->p;
om_ib[1][0] = sensorData_ptr->imuData_ptr->q;
om_ib[2][0] = sensorData_ptr->imuData_ptr->r;
}