/*--------------------------------------------------------------------*/
MATRIX calcTasUBFromTwoReflections(lattice cell, tasReflection r1,
tasReflection r2, int *errorCode){
MATRIX B, HT, UT, U, UB, HTT ;
MATRIX u1, u2, h1, h2, planeNormal;
double ud[3];
int status;
*errorCode = 1;
/*
calculate the B matrix and the HT matrix
*/
B = mat_creat(3,3,ZERO_MATRIX);
status = calculateBMatrix(cell,B);
if(status < 0){
*errorCode = status;
return NULL;
}
h1 = tasReflectionToHC(r1.qe,B);
h2 = tasReflectionToHC(r2.qe,B);
if(h1 == NULL || h2 == NULL){
*errorCode = UBNOMEMORY;
mat_free(B);
return NULL;
}
HT = matFromTwoVectors(h1,h2);
if(HT == NULL){
*errorCode = UBNOMEMORY;
mat_free(B);
mat_free(h1);
mat_free(h2);
return NULL;
}
/*
calculate U vectors and UT matrix
*/
u1 = calcTasUVectorFromAngles(r1);
u2 = calcTasUVectorFromAngles(r2);
if(u1 == NULL || u2 == NULL){
*errorCode = UBNOMEMORY;
mat_free(B);
mat_free(h1);
mat_free(h2);
return NULL;
}
UT = matFromTwoVectors(u1,u2);
if(UT == NULL){
*errorCode = UBNOMEMORY;
mat_free(B);
mat_free(h1);
mat_free(h2);
mat_free(u1);
mat_free(u2);
mat_free(HT);
return NULL;
}
/*
UT = U * HT
*/
HTT = mat_tran(HT);
if(HTT == NULL){
*errorCode = UBNOMEMORY;
mat_free(B);
mat_free(h1);
mat_free(h2);
mat_free(u1);
mat_free(u2);
mat_free(HT);
return NULL;
}
U = mat_mul(UT,HTT);
if(U == NULL){
*errorCode = UBNOMEMORY;
mat_free(B);
mat_free(h1);
mat_free(h2);
mat_free(u1);
mat_free(u2);
mat_free(HT);
mat_free(HTT);
return NULL;
}
UB = mat_mul(U,B);
if(UB == NULL){
mat_free(B);
mat_free(h1);
mat_free(h2);
mat_free(u1);
mat_free(u2);
mat_free(HT);
mat_free(HTT);
mat_free(U);
*errorCode = UBNOMEMORY;
}
/*
clean up
*/
killVector(h1);
killVector(h2);
mat_free(HT);
mat_free(HTT);
killVector(u1);
killVector(u2);
mat_free(UT);
mat_free(U);
mat_free(B);
return UB;
}
// 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;
}
