MATRIX mat_cart2pol(MATRIX A, int dim, MATRIX result)
{
int m, n, i;
m = MatCol(A);
n = MatRow(A);
if(dim==0 && n>1)
{
if(result==NULL)if((result = mat_creat(2, m, ZERO_MATRIX))==NULL) return NULL;
for(i=0; i<m; ++i)
__cart2pol(A[0][i], A[1][i], result[0][i], result[1][i]);
return result;
}
if(dim==1 && m>1)
{
if(result==NULL)if((result = mat_creat(n, 2, ZERO_MATRIX))==NULL) return NULL;
for(i=0; i<n; ++i)
__cart2pol(A[i][0], A[i][1], result[i][0], result[i][1]);
return result;
}
return mat_error (MAT_SIZEMISMATCH);
}
MATRIX mat_get_sub_matrix_from_cols(MATRIX A, INT_VECTOR indices, MATRIX result)
{
int i, j, k, n;
k = MatRow(A);
n = Int_VecLen(indices);
if(result==NULL) if((result = mat_creat(k, n, UNDEFINED))==NULL)
return mat_error(MAT_MALLOC);
for(i=0; i<n; ++i)
{
for(j=0; j<k; ++j) result[j][i] = A[j][indices[i]];
}
return result;
}
MATRIX mat_sum_col(MATRIX A, MATRIX result)
{
int i, j, m, n;
m = MatCol(A);
n = MatRow(A);
if(result==NULL) if((result = mat_creat(1, m, ZERO_MATRIX))==NULL) mat_error(MAT_MALLOC);
#pragma omp parallel for private(j)
for(i=0; i<m; ++i)
{
result[0][i] = 0.0;
for(j=0; j<n; ++j) result[0][i] += A[j][i];
}
return result;
}
MATRIX mat_creat_diag(MATRIX diag_vals, MATRIX result)
{
int i, sz;
if(MatCol(diag_vals)==1) sz = MatRow(diag_vals);
else sz = MatCol(diag_vals);
if(result==NULL) if((result = mat_creat(sz, sz, ZERO_MATRIX))==NULL) mat_error(MAT_MALLOC);
{
if(MatCol(diag_vals)==1)
for(i=0; i<sz; ++i) result[i][i] = diag_vals[i][0];
else
for(i=0; i<sz; ++i) result[i][i] = diag_vals[0][i];
return result;
}
}
/*
*-----------------------------------------------------------------------------
* funct: mat_minor
* desct: find minor
* given: A = a square matrix,
* i=row, j=col
* retrn: the minor of Aij
*-----------------------------------------------------------------------------
*/
double mat_minor(MATRIX A,int i,int j)
{
MATRIX S;
double result;
if ( ( S = mat_creat(MatRow(A)-1, MatCol(A)-1, UNDEFINED) ) == NULL )
return -1.0;
mat_submat(A, i, j, S);
result = mat_det( S );
mat_free(S);
return (result);
}
MATRIX mat_randperm(int m, int n, MATRIX result)
{
int i, j;
MATRIX tmp = NULL;
if(result==NULL) if((result = mat_creat(m, n, UNDEFINED))==NULL)
return mat_error(MAT_MALLOC);
for(i=0; i<m; ++i)
{
tmp = mat_randperm_n(n, tmp);
for(j=0; j<n; ++j) result[i][j] = tmp[0][j];
}
mat_free(tmp);
return result;
}
MATRIX mat_tran(MATRIX A, MATRIX result)
{
int i, j, m, n;
m = MatCol(A);
n = MatRow(A);
if(result== NULL) if((result = mat_creat(m,n, UNDEFINED))==NULL)
return mat_error(MAT_MALLOC);
for(i=0; i<m; ++i)
for (j=0; j<n; ++j)
{
result[i][j] = A[j][i];
}
return result;
}
MATRIX mat_copy(MATRIX A, MATRIX result)
{
int i, j, m, n;
m = MatCol(A);
n = MatRow(A);
if(result==NULL) if((result = mat_creat(n, m, UNDEFINED))==NULL)
return mat_error(MAT_MALLOC);
#pragma omp parallel for private(j)
for(i=0; i<n; ++i)
for(j=0; j<m; ++j)
{
result[i][j] = A[i][j];
}
return result;
}
MATRIX mat_randfun(int n, int m, mtype (*fun)(mtype), mtype xmin, mtype xmax, MATRIX result)
{
int i, j;
if(result==NULL) if((result = mat_creat(n, m, UNDEFINED))==NULL)
return mat_error(MAT_MALLOC);
if(!MAT_SET_SEED)mat_set_seed(0);
for(i=0; i<n; ++i)
{
for(j=0; j<m; ++j)
{
result[i][j] = __mat_randfun(fun, xmin, xmax);
}
}
return result;
}
MATSTACK __matstack_creat(int len)
{
MATSTACK matrixstack;
int i;
if((matrixstack = (MATRIX *)malloc(sizeof(MATRIX)*(1+len)))==NULL)
return (matstack_error( MATSTACK_MALLOC));
for(i=0; i<=len; ++i)
{
matrixstack[i]= NULL;
}
matrixstack[0] = mat_creat(1,1,0);
matrixstack[0][0][0] = len;
return (matrixstack+1);
}
MATRIX mat_rand(int n, int m, MATRIX result)
{
int i, j;
if(result==NULL) if((result = mat_creat(n, m, UNDEFINED))==NULL)
return mat_error(MAT_MALLOC);
if(!MAT_SET_SEED)mat_set_seed(0);
for(i=0; i<n; ++i)
{
for(j=0; j<m; ++j)
{
result[i][j] = ((mtype)rand())/((mtype)RAND_MAX+1.0);
}
}
return result;
}
MATRIX mat_xcopy(MATRIX A, int si, int ei, int sj, int ej, MATRIX result)
{
int i, j, m, n;
m = MatCol(A);
n = MatRow(A);
if(si<0 || sj<0 || ei>n || ei>m) mat_error(MAT_SIZEMISMATCH);
if(result== NULL) if((result = mat_creat(ei-si, ej-sj, UNDEFINED))==NULL)
return mat_error(MAT_MALLOC);
#pragma omp parallel for private(j)
for(i=si; i<ei; ++i)
for(j=sj; j<ej; ++j)
{
result[i-si][j-sj] = A[i][j];
}
return result;
}
//---------------------------------------------------------------------------
// 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 */
}
MATRIX mat_div_dot(MATRIX A, MATRIX B, MATRIX result)
{
int i, j, m, n, o, p;
m = MatCol(A);
n = MatRow(A);
o = MatCol(B);
p = MatRow(B);
if(result==NULL) if((result = mat_creat(MatRow(A), MatCol(A), UNDEFINED))==NULL)
return mat_error(MAT_MALLOC);
if(o==m &&p==n)
{
#pragma omp parallel for private(j)
for(i=0; i<n; ++i)
{
for(j=0; j<m; ++j)
{
result[i][j] = A[i][j]/B[i][j];
}
}
}
else if(o==1 && p!=1)
{
#pragma omp parallel for private(j)
for(i=0; i<n; ++i)
{
for(j=0; j<m; ++j)
{
result[i][j] = A[i][j]/B[i][0];
}
}
}
else if(p==1 && o!=1)
{
#pragma omp parallel for private(j)
for(i=0; i<n; ++i)
{
for(j=0; j<m; ++j)
{
result[i][j] = A[i][j]/B[0][j];
}
}
}
else gen_error(GEN_SIZEMISMATCH);
return result;
}
MATRIX mat_randperm_n(int n, MATRIX result)
{
int i, j;
mtype t = 0.0;
if(result==NULL) if((result = mat_creat(1, n, UNDEFINED))==NULL)
return mat_error(MAT_MALLOC);
if(!MAT_SET_SEED) mat_set_seed(0);
for(i=0; i<n; ++i)
result[0][i] = i;
for(i=0; i<n; ++i)
{
j = rand()%(n-i)+i;
t = result[0][j];
result[0][j] =result[0][i];
result[0][i] = t;
}
return result;
}
MATRIX mat_calc_dist_sq(MATRIX A, MATRIX d, MATRIX result)
{
int i, j, m, n;
mtype dist;
m = MatRow(A);
n = MatCol(A);
if(result==NULL) if((result = mat_creat(1, n, ZERO_MATRIX))==NULL) return mat_error(MAT_MALLOC);;
for(i=0; i<n; ++i)
{
dist = 0.0;
for(j=0; j<m; ++j)
{
dist += (A[j][i]-d[j][0])*(A[j][i]-d[j][0]);
}
result[0][i] = dist;
}
return result;
}
/*
*-----------------------------------------------------------------------------
* funct: mat_lsolve_durbin
* desct: Solve simultaneous linear eqns using
* Levinson-Durbin algorithm
*
* This function solve the linear eqns Ax = B:
*
* | v0 v1 v2 .. vn-1 | | a1 | | v1 |
* | v1 v0 v1 .. vn-2 | | a2 | | v2 |
* | v2 v1 v0 .. vn-3 | | a3 | = | .. |
* | ... | | .. | | .. |
* | vn-1 vn-2 .. .. v0 | | an | | vn |
*
* domain: where A is a symmetric Toeplitz matrix and B
* in the above format (related to A)
*
* given: A, B
* retrn: x (of Ax = B)
*
*-----------------------------------------------------------------------------
*/
MATRIX mat_lsolve_durbin(MATRIX A,MATRIX B,MATRIX X)
{
MATRIX R;
int i, n;
n = MatRow(A);
if ( ( R = mat_creat(n+1, 1, UNDEFINED) ) == NULL )
return (NULL);
for (i=0; i<n; i++)
{
R[i][0] = A[i][0];
}
R[n][0] = B[n-1][0];
mat_durbin( R, X );
mat_free( R );
return(X);
}
MATRIX mat_scpcol(MATRIX data)
{
int i, j1, j2, m, n;
MATRIX spco = NULL;
m = MatCol(data);
n = MatRow(data);
spco = mat_creat(m, m, ZERO_MATRIX);
for(j1=0; j1<m; ++j1)
{
for(j2=j1; j2<m; ++j2)
{
spco[j1][j2] = 0.0;
for(i=0; i<n; ++i)
{
spco[j1][j2] += data[i][j1]*data[i][j2];
}
spco[j2][j1] = spco[j1][j2];
}
}
return spco;
}
/*-----------------------------------------------------------------------------*/
static MATRIX buildTVMatrix(MATRIX U1V, MATRIX U2V){
MATRIX T, T3V;
int i;
normalizeVector(U2V);
T3V = vectorCrossProduct(U1V,U2V);
normalizeVector(T3V);
if(T3V == NULL){
return NULL;
}
T = mat_creat(3,3,ZERO_MATRIX);
if(T == NULL){
killVector(T3V);
return NULL;
}
for(i = 0; i < 3; i++){
T[i][0] = U1V[i][0];
T[i][1] = U2V[i][0];
T[i][2] = T3V[i][0];
}
killVector(T3V);
return T;
}
/*--------------------------------------------------------------------*/
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;
}
MATRIX ortho(MATRIX C, MATRIX C_ortho)
{
/*
This function orthogonalizes a DCM by method presented in the paper
Bar-Itzhack: "Orthogonalization Techniques of DCM" (1969, IEEE)
Input:
C: DCM, 3x3
Output:
C_ortho: Orthogonalized DCM, 3x3
Programmer: Adhika Lie
Created: May 10, 2011
Last Modified: May 10, 2011
*/
MATRIX e = mat_creat(3,1,ONES_MATRIX);
MATRIX w1 = mat_creat(3,1,ONES_MATRIX);
MATRIX w1_p = mat_creat(3,1,ONES_MATRIX);
MATRIX w1_n = mat_creat(3,1,ONES_MATRIX);
MATRIX w2 = mat_creat(3,1,ONES_MATRIX);
MATRIX w2_p = mat_creat(3,1,ONES_MATRIX);
MATRIX w2_n = mat_creat(3,1,ONES_MATRIX);
MATRIX w3 = mat_creat(3,1,ONES_MATRIX);
MATRIX w3_p = mat_creat(3,1,ONES_MATRIX);
MATRIX w3_n = mat_creat(3,1,ONES_MATRIX);
double mag_w1, mag_w2, mag_w3;
w1[0][0] = C[0][0];
w1[1][0] = C[1][0];
w1[2][0] = C[2][0];
mag_w1 = norm(w1);
mag_w1 = 1.0/mag_w1;
w1 = mat_scalMul(w1,mag_w1,w1);
w2[0][0] = C[0][1];
w2[1][0] = C[1][1];
w2[2][0] = C[2][1];
mag_w2 = norm(w2);
mag_w2 = 1.0/mag_w2;
w2 = mat_scalMul(w2,mag_w2,w2);
w3[0][0] = C[0][2];
w3[1][0] = C[1][2];
w3[2][0] = C[2][2];
mag_w3 = norm(w3);
mag_w3 = 1.0/mag_w3;
w3 = mat_scalMul(w3,mag_w3,w3);
while (norm(e) > 1e-15){
w1_p = cross(w2,w3,w1_p);
w2_p = cross(w3,w1,w2_p);
w3_p = cross(w1,w2,w3_p);
w1_n = mat_add(w1,w1_p,w1_n);
w1_n = mat_scalMul(w1_n,0.5,w1_n);
w1 = mat_scalMul(w1_n,1.0/norm(w1_n),w1);
w2_n = mat_add(w2,w2_p,w2_n);
w2_n = mat_scalMul(w2_n,0.5,w2_n);
w2 = mat_scalMul(w2_n,1.0/norm(w2_n),w2);
w3_n = mat_add(w3,w3_p,w3_n);
w3_n = mat_scalMul(w3_n,0.5,w3_n);
w3 = mat_scalMul(w3_n,1.0/norm(w3_n),w3);
w1_p = cross(w2,w3,w1_p);
w2_p = cross(w3,w1,w2_p);
w3_p = cross(w1,w2,w3_p);
w1_n = mat_sub(w1,w1_p,w1_n);
e[0][0] = norm(w1_n);
w2_n = mat_sub(w2,w2_p,w2_n);
e[1][0] = norm(w2_n);
w3_n = mat_sub(w3,w3_p,w3_n);
e[2][0] = norm(w3_n);
}
C_ortho[0][0] = w1[0][0]; C_ortho[0][1] = w2[0][0]; C_ortho[0][2] = w3[0][0];
C_ortho[1][0] = w1[1][0]; C_ortho[1][1] = w2[1][0]; C_ortho[1][2] = w3[1][0];
C_ortho[2][0] = w1[2][0]; C_ortho[2][1] = w2[2][0]; C_ortho[2][2] = w3[2][0];
return C_ortho;
}
MATRIX mat_bsxfun(MATRIX A, MATRIX B, mtype (*func)(mtype, mtype), MATRIX result)
{
int m, n, o, p, i, j;
m = MatRow(A);
n = MatCol(A);
o = MatRow(B);
p = MatCol(B);
if(m<o && n==p && m==1)
{
if(result== NULL) if((result = mat_creat(o, n, UNDEFINED))==NULL) return mat_error(MAT_MALLOC);
for(i=0; i<o; ++i)
{
for(j=0; j<n; ++j)
{
result[i][j] = (*func)(A[0][j], B[i][j]);
}
}
}
else if(m>o && n==p && o==1)
{
if(result== NULL) if((result = mat_creat(m, n, UNDEFINED))==NULL) return mat_error(MAT_MALLOC);
for(i=0; i<m; ++i)
{
for(j=0; j<n; ++j)
{
result[i][j] = (*func)(A[i][j], B[0][j]);
}
}
}
else if(m==o && n<p && n==1)
{
if(result== NULL) if((result = mat_creat(m, p, UNDEFINED))==NULL) return mat_error(MAT_MALLOC);
for(i=0; i<m; ++i)
{
for(j=0; j<p; ++j)
{
result[i][j] = (*func)(A[i][0], B[i][j]);
}
}
}
else if(m==o && n>p && p==1)
{
if(result== NULL) if((result = mat_creat(m, n, UNDEFINED))==NULL) return mat_error(MAT_MALLOC);
for(i=0; i<m; ++i)
{
for(j=0; j<n; ++j)
{
result[i][j] = (*func)(A[i][j], B[i][0]);
}
}
}
else if(m==1 && p==1)
{
if(result== NULL) if((result = mat_creat(o, n, UNDEFINED))==NULL) return mat_error(MAT_MALLOC);
for(i=0; i<o; ++i)
{
for(j=0; j<n; ++j)
{
result[i][j] = (*func)(A[0][j], B[i][0]);
}
}
}
else if(n==1 && o==1)
{
if(result== NULL) if((result = mat_creat(m, p, UNDEFINED))==NULL) return mat_error(MAT_MALLOC);
for(i=0; i<m; ++i)
{
for(j=0; j<p; ++j)
{
result[i][j] = (*func)(A[i][0], B[0][j]);
}
}
}
else mat_error(MAT_SIZEMISMATCH);
return result;
}
extern void get_guidance(double time, struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr, struct mission *missionData_ptr){
if (time>0.05){
#ifdef AIRCRAFT_THOR
controlData_ptr->ias_cmd = 17; //Trim airspeed (m/s)
#endif
#ifdef AIRCRAFT_TYR
controlData_ptr->ias_cmd = 17; //Trim airspeed (m/s)
#endif
#ifdef AIRCRAFT_FASER
controlData_ptr->ias_cmd = 23;
#endif
#ifdef AIRCRAFT_IBIS
controlData_ptr->ias_cmd = 23;
#endif
#ifdef AIRCRAFT_BALDR
controlData_ptr->ias_cmd = 23;
#endif
// Initialization of algorithm and variables
if (guide_init==0) // init variables
{
pos_lla = mat_creat(3,1,ZERO_MATRIX); // LAT, LON, alt vector
pos_ecef = mat_creat(3,1,ZERO_MATRIX); // ECEF position vector
pos_ecef0 = mat_creat(3,1,ZERO_MATRIX); // ECEF position vector of initial point
v_ned = mat_creat(3,1,ZERO_MATRIX); // vector of NED velocity
pos_ned = mat_creat(3,1,ZERO_MATRIX); // vector of NED position
tmp31 = mat_creat(3,1,ZERO_MATRIX); // temporary 3x1 vector
T_ecef2ned = mat_creat(3,3,ZERO_MATRIX); // transformation matrix from ECEF to NED
WP_goal = mat_creat(2,1,ZERO_MATRIX); // Vector of North / East goal coordinates (actaul waypoint)
guide_init=1; // guidance initialization finished
}
if (guide_start==0)
{
pos_lla[0][0]=navData_ptr->lat;
pos_lla[1][0]=navData_ptr->lon;
pos_lla[2][0]=navData_ptr->alt;
// transform starting point from LLA to ECEF
LatLonAltToEcef2(pos_ecef0, pos_lla);
// transformation matrix for LaunchPoint ECEF |--> NED for initial point
T_ecef2ned[0][0] = -sin(pos_lla[0][0])*cos(pos_lla[1][0]); T_ecef2ned[0][1] = -sin(pos_lla[0][0])*sin(pos_lla[1][0]); T_ecef2ned[0][2] = cos(pos_lla[0][0]);
T_ecef2ned[1][0] = -sin(pos_lla[1][0]); T_ecef2ned[1][1] = cos(pos_lla[1][0]); T_ecef2ned[1][2] = 0.0;
T_ecef2ned[2][0] = -cos(pos_lla[0][0])*cos(pos_lla[1][0]); T_ecef2ned[2][1] = -cos(pos_lla[0][0])*sin(pos_lla[1][0]); T_ecef2ned[2][2] = -sin(pos_lla[0][0]);
//save next waypoint coordinates
WP_goal[0][0]=waypoints[nextwaypoint][0];
WP_goal[1][0]=waypoints[nextwaypoint][1];
guide_start=1;
}
else
cpsi=0; //to handle SIL initial zero data
// current vehicle position/vel (computed at every iteration)
pos_lla[0][0]=navData_ptr->lat;
pos_lla[1][0]=navData_ptr->lon;
pos_lla[2][0]=navData_ptr->alt;
v_ned[0][0] = navData_ptr->vn;
v_ned[1][0] = navData_ptr->ve;
v_ned[2][0] = navData_ptr->vd;
// azimuth angle from ground speed
psi=atan2(v_ned[1][0], v_ned[0][0]);
// Transfrom the difference of actual coordinates from the starting point into NED frame
LatLonAltToEcef2(pos_ecef, pos_lla);
mat_sub(pos_ecef, pos_ecef0, tmp31);
mat_mul(T_ecef2ned, tmp31, pos_ned);
// Calculate waypoint distance from actual point
d2WP=distance2waypoint(WP_goal, pos_ned);
// change of target waypoint: check if vehicle is within x meters of the target waypoint
if ((d2WP < WPTOL) && (guide_start==1)) {
if(nextwaypoint == numofwaypoints-1) nextwaypoint = 0; // change back to first waypoint
else nextwaypoint++;
// select new waypoint parameters
WP_goal[0][0] = waypoints[nextwaypoint][0];
WP_goal[1][0] = waypoints[nextwaypoint][1];
pinit=0;
}
// actual aircraft position
xA=pos_ned[0][0]; // North
yA=pos_ned[1][0]; // East
// Target coordinates
xT=WP_goal[0][0];
yT=WP_goal[1][0];
if (pinit==0) // decide about tracking method
{
if (sensorData_ptr->adData_ptr->ias_filt>10) // only for SIL because nonzero initial conditions
{
psiT=atan2(yT-yA, xT-xA); // Aircraft target azimuth angle
// azimuth angle correction if required:
if (fabs(psiT-wraparound(psi))>PI)
psiT=psiT+mysign(wraparound(psi))*PI2;
Rt=pow(controlData_ptr->ias_cmd, 2)/g/tan(PHI0); // possible turn radius
// center of circle:
dpsi=mysign(psiT-wraparound(psi))*90*D2R;
xC=xA+Rt*cos(psi+dpsi);
yC=yA+Rt*sin(psi+dpsi);
// check feasibility
R2=sqrt(pow(WP_goal[0][0]-xC, 2)+pow(WP_goal[1][0]-yC, 2)); // distance of waypoint from circle center
if (R2<Rt+FTOL) // infeasible problem turn into opposite direction
{
cpsi=-dpsi*4;
lc=8;
pinit=1;
}
else // feasible problem, go to waypoint
{
cpsi=psiT-wraparound(psi);
lc=0;
pinit=1;
}
} // end if nonzero velocity
} // end decide about tracking method
if (lc==8) // check if the algorithm can change to circular segment tracking
{
// calculate actual circle parameters
Rt=pow(controlData_ptr->ias_cmd, 2)/g/tan(PHI0); // turn radius
// center of circle:
psiT=atan2(yT-yA, xT-xA);
// azimuth angle correction if required:
if (fabs(psiT-wraparound(psi))>PI)
psiT=psiT+mysign(wraparound(psi))*PI2;
// center of circle:
dpsi=mysign(psiT-wraparound(psi))*90*D2R;
xC=xA+Rt*cos(psi+dpsi);
yC=yA+Rt*sin(psi+dpsi);
// check feasibility
R2=sqrt(pow(WP_goal[0][0]-xC, 2)+pow(WP_goal[1][0]-yC, 2));
if (R2>=Rt+FTOL) // fasible problem, track circle
{
cpsi=psiT-wraparound(psi);
lc=0;
}
else
cpsi=-dpsi*4;
}
else if (lc==0) {
psiT=atan2(yT-yA, xT-xA);
// azimuth angle correction if required:
if (fabs(psiT-wraparound(psi))>PI)
psiT=psiT+mysign(wraparound(psi))*PI2;
cpsi=psiT-wraparound(psi);
}
// send out control command
controlData_ptr->psi_cmd=cpsi;
// send out diagnostic variable
controlData_ptr->r_cmd=((nextwaypoint+1)*10+lc);
}
}
// make sure C is 3 x 3
MATRIX mat_T321 (double pitch, double roll, double yaw, MATRIX C)
{
MATRIX TI2,T23,T3B,t1;
double cphi,sphi,ctheta,stheta,cpsi,spsi;
// if dimensions of C is wrong
if ( MatCol(C) != 3 || MatRow(C) != 3 ) {
printf("mat_T321 error: incompatible output matrix size\n");
_exit(-1);
// if dimensions of C is correct
} else {
if ((TI2 = mat_creat( 3, 3, UNDEFINED )) == NULL)
return (NULL);
if ((T23 = mat_creat( 3, 3, UNDEFINED )) == NULL)
return (NULL);
if ((T3B = mat_creat( 3, 3, UNDEFINED )) == NULL)
return (NULL);
cphi = cos(roll);
sphi = sin(roll);
ctheta = cos(pitch);
stheta = sin(pitch);
cpsi = cos(yaw);
spsi = sin(yaw);
TI2[0][0] = spsi;
TI2[0][1] = cpsi;
TI2[0][2] = 0;
TI2[1][0] = cpsi;
TI2[1][1] = -spsi;
TI2[1][2] = 0;
TI2[2][0] = 0;
TI2[2][1] = 0;
TI2[2][2] = -1;
T23[0][0] = ctheta;
T23[0][1] = 0;
T23[0][2] = -stheta;
T23[1][0] = 0;
T23[1][1] = 1;
T23[1][2] = 0;
T23[2][0] = stheta;
T23[2][1] = 0;
T23[2][2] = ctheta;
T3B[0][0] = 1;
T3B[0][1] = 0;
T3B[0][2] = 0;
T3B[1][0] = 0;
T3B[1][1] = cphi;
T3B[1][2] = sphi;
T3B[2][0] = 0;
T3B[2][1] = -sphi;
T3B[2][2] = cphi;
// C = T3B*T23*TI2;
if ( ( t1 = mat_creat(3,3,UNDEFINED) ) == NULL )
return (NULL);
mat_mul(T23,TI2,t1);
mat_mul(T3B,t1, C);
}
mat_free(T3B);
mat_free(T23);
mat_free(TI2);
mat_free(t1);
return(C);
}
void init_nav(struct imu *imuData_ptr, struct gps *gpsData_ptr, struct nav *navData_ptr){
/*++++++++++++++++++++++++++++++++++++++++++++++++
*matrix creation for navigation computation
*++++++++++++++++++++++++++++++++++++++++++++++++*/
F = mat_creat(15,15,ZERO_MATRIX); // State Matrix
PHI = mat_creat(15,15,ZERO_MATRIX); // State transition Matrix
P = mat_creat(15,15,ZERO_MATRIX); // Covariance Matrix
G = mat_creat(15,12,ZERO_MATRIX); // for Process Noise Transformation
Rw = mat_creat(12,12,ZERO_MATRIX);
Qw = mat_creat(15,15,ZERO_MATRIX);
Q = mat_creat(15,15,ZERO_MATRIX); // Process Noise Matrix
R = mat_creat(6,6,ZERO_MATRIX); // GPS Measurement Noise matrix
x = mat_creat(15,1,ZERO_MATRIX);
y = mat_creat(6,1,ZERO_MATRIX); // GPS Measurement
eul = mat_creat(3,1,ZERO_MATRIX);
Rbodtonav = mat_creat(3,3,ZERO_MATRIX);
grav = mat_creat(3,1,ZERO_MATRIX); // Gravity Model
f_b = mat_creat(3,1,ZERO_MATRIX);
om_ib = mat_creat(3,1,ZERO_MATRIX);
nr = mat_creat(3,1,ZERO_MATRIX); // Nav Transport Rate
K = mat_creat(15,6,ZERO_MATRIX); // Kalman Gain
H = mat_creat(6,15,ZERO_MATRIX);
C_N2B = mat_creat(3,3,ZERO_MATRIX); // DCM
C_B2N = mat_creat(3,3,ZERO_MATRIX); // DCM Transpose
pos_ref = mat_creat(3,1,ZERO_MATRIX);
pos_ins_ecef= mat_creat(3,1,ZERO_MATRIX);
pos_ins_ned = mat_creat(3,1,ZERO_MATRIX);
pos_gps = mat_creat(3,1,ZERO_MATRIX);
pos_gps_ecef= mat_creat(3,1,ZERO_MATRIX);
pos_gps_ned = mat_creat(3,1,ZERO_MATRIX);
I15 = mat_creat(15,15,UNIT_MATRIX); // Identity
I3 = mat_creat(3,3,UNIT_MATRIX); // Identity
ImKH = mat_creat(15,15,ZERO_MATRIX);
KRKt = mat_creat(15,15,ZERO_MATRIX);
dx = mat_creat(3,1,ZERO_MATRIX); // Temporary to get dxdt
a_temp31 = mat_creat(3,1,ZERO_MATRIX); // Temporary
b_temp31 = mat_creat(3,1,ZERO_MATRIX); // Temporary
temp33 = mat_creat(3,3,ZERO_MATRIX); // Temporary
atemp33 = mat_creat(3,3,ZERO_MATRIX); // Temporary
temp1515 = mat_creat(15,15,ZERO_MATRIX);
temp615 = mat_creat(6,15,ZERO_MATRIX);
temp66 = mat_creat(6,6,ZERO_MATRIX);
atemp66 = mat_creat(6,6,ZERO_MATRIX);
temp156 = mat_creat(15,6,ZERO_MATRIX);
temp1512 = mat_creat(15,12,ZERO_MATRIX);
// Assemble the matrices
// .... gravity, g
grav[2][0] = g;
// ... H
H[0][0] = 1.0; H[1][1] = 1.0; H[2][2] = 1.0;
H[3][3] = 1.0; H[4][4] = 1.0; H[5][5] = 1.0;
// ... Rw
Rw[0][0] = SIG_W_AX*SIG_W_AX; Rw[1][1] = SIG_W_AY*SIG_W_AY; Rw[2][2] = SIG_W_AZ*SIG_W_AZ;
Rw[3][3] = SIG_W_GX*SIG_W_GX; Rw[4][4] = SIG_W_GY*SIG_W_GY; Rw[5][5] = SIG_W_GZ*SIG_W_GZ;
Rw[6][6] = 2*SIG_A_D*SIG_A_D/TAU_A; Rw[7][7] = 2*SIG_A_D*SIG_A_D/TAU_A; Rw[8][8] = 2*SIG_A_D*SIG_A_D/TAU_A;
Rw[9][9] = 2*SIG_G_D*SIG_G_D/TAU_G; Rw[10][10] = 2*SIG_G_D*SIG_G_D/TAU_G; Rw[11][11] = 2*SIG_G_D*SIG_G_D/TAU_G;
// ... P (initial)
P[0][0] = P_P_INIT*P_P_INIT; P[1][1] = P_P_INIT*P_P_INIT; P[2][2] = P_P_INIT*P_P_INIT;
P[3][3] = P_V_INIT*P_V_INIT; P[4][4] = P_V_INIT*P_V_INIT; P[5][5] = P_V_INIT*P_V_INIT;
P[6][6] = P_A_INIT*P_A_INIT; P[7][7] = P_A_INIT*P_A_INIT; P[8][8] = P_HDG_INIT*P_HDG_INIT;
P[9][9] = P_AB_INIT*P_AB_INIT; P[10][10] = P_AB_INIT*P_AB_INIT; P[11][11] = P_AB_INIT*P_AB_INIT;
P[12][12] = P_GB_INIT*P_GB_INIT; P[13][13] = P_GB_INIT*P_GB_INIT; P[14][14] = P_GB_INIT*P_GB_INIT;
// ... update P in get_nav
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];
// ... R
R[0][0] = SIG_GPS_P_NE*SIG_GPS_P_NE; R[1][1] = SIG_GPS_P_NE*SIG_GPS_P_NE; R[2][2] = SIG_GPS_P_D*SIG_GPS_P_D;
R[3][3] = SIG_GPS_V*SIG_GPS_V; R[4][4] = SIG_GPS_V*SIG_GPS_V; R[5][5] = SIG_GPS_V*SIG_GPS_V;
// .. then initialize states with GPS Data
navData_ptr->lat = gpsData_ptr->lat*D2R;
navData_ptr->lon = gpsData_ptr->lon*D2R;
navData_ptr->alt = gpsData_ptr->alt;
navData_ptr->vn = gpsData_ptr->vn;
navData_ptr->ve = gpsData_ptr->ve;
navData_ptr->vd = gpsData_ptr->vd;
// ... and initialize states with IMU Data
//navData_ptr->the = asin(imuData_ptr->ax/g); // theta from Ax, aircraft at rest
//navData_ptr->phi = asin(-imuData_ptr->ay/(g*cos(navData_ptr->the))); // phi from Ay, aircraft at rest
navData_ptr->the = 8*D2R;
navData_ptr->phi = 0*D2R;
navData_ptr->psi = 90.0*D2R;
eul2quat(navData_ptr->quat,(navData_ptr->phi),(navData_ptr->the),(navData_ptr->psi));
navData_ptr->ab[0] = 0.0;
navData_ptr->ab[1] = 0.0;
navData_ptr->ab[2] = 0.0;
navData_ptr->gb[0] = imuData_ptr->p;
navData_ptr->gb[1] = imuData_ptr->q;
navData_ptr->gb[2] = imuData_ptr->r;
// Specific forces and Rotation Rate
f_b[0][0] = imuData_ptr->ax - navData_ptr->ab[0];
f_b[1][0] = imuData_ptr->ay - navData_ptr->ab[1];
f_b[2][0] = imuData_ptr->az - navData_ptr->ab[2];
om_ib[0][0] = imuData_ptr->p - navData_ptr->gb[0];
om_ib[1][0] = imuData_ptr->q - navData_ptr->gb[1];
om_ib[2][0] = imuData_ptr->r - navData_ptr->gb[2];
// Time during initialization
tprev = imuData_ptr->time;
navData_ptr->err_type = data_valid;
//send_status("NAV filter initialized");
//fprintf(stderr,"NAV Data Initialization Completed\n");
}
//---------------------------------------------------------------------------
// Compute determinant of the matrix
NaReal NaMatrix::det () const
{
#ifdef __matrix_h
if(dim_rows() != dim_cols()){
throw(na_size_mismatch);
}
NaReal vDet = 0;
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);
}
}
vDet = mat_det(mat);
mat_free(mat);
return vDet;
#else
throw(na_not_implemented);
#endif/* __matrix_h */
#if 0
unsigned i, j, k, n = dim_rows();
// Predefined det()
switch(n){
case 1:
vDet = get(0, 0);
break;
case 2:
vDet = get(0, 0) * get (1, 1) - get(0, 1) * get(1, 0);
break;
default:{
// Allocate space for minor matrix
NaMatrix mOfMinor(n - 1, n - 1);
for(i = 0; i < n; ++i){
// Compose minor
for(j = 0; j < n - 1; ++j){
for(k = 0; k < n - 1; ++k){
if(k < i){
mOfMinor[j][k] = get(1 + j, k);
}else if(k >= i){
mOfMinor[j][k] = get(1 + j, 1 + k);
}
}
}// minor composition
//NaPrintLog("Minor(%d,%d):\n", i, n - 1);
//mOfMinor.print_contents();
// Summarize determinant
if(i % 2){
vDet -= get(0, i) * mOfMinor.det();
}else{
vDet += get(0, i) * mOfMinor.det();
}
}
}break;
}// end of switch for different cases
//NaPrintLog("det=%g\n", vDet);
return vDet;
#endif
}
MATSTACK mat_pca(MATRIX data, int pca_type)
{
int i, j, k, k2, m, n;
MATRIX evals, im;
MATSTACK tmmps0 = NULL;
MATRIX symmat, symmat2;
m = MatCol(data);
n = MatRow(data);
switch(pca_type)
{
case MAT_PCA_CORRELATION:
tmmps0 = mat_corcol(data);
symmat = tmmps0[1];
break;
case MAT_PCA_COVARIANCE:
tmmps0 = mat_covcol(data);
symmat = tmmps0[1];
break;
case MAT_PCA_SUMOFSQUARES:
symmat = mat_scpcol(data);
break;
default:
tmmps0 = mat_covcol(data);
symmat = tmmps0[1];
break;
}
evals = mat_creat(m, 1, UNDEFINED);
im = mat_creat(m, 1, UNDEFINED);
symmat2 = mat_copy(symmat, NULL);
mat_tred2(symmat, evals, im);
mat_tqli(evals, im, symmat);
for(i=0; i<n; ++i)
{
for(j=0; j<m; ++j)
{
im[j][0] = tmmps0[2][i][j];
}
for(k=0; k<3; ++k)
{
tmmps0[2][i][k] = 0.0;
for(k2=0; k2<m; ++k2)
{
tmmps0[2][i][k] += im[k2][0] * symmat[k2][m-k-1];
}
}
}
for(j=0; j<m; ++j)
{
for(k=0; k<m; ++k)
{
im[k][0] = symmat2[j][k];
}
for(i=0; i<3; ++i)
{
symmat2[j][i] = 0.0;
for (k2=0; k2<m; ++k2)
{
symmat2[j][i] += im[k2][0] * symmat[k2][m-i-1];
}
if(evals[m-i-1][0]>0.0005)
symmat2[j][i] /= (mtype)sqrt(evals[m-i-1][0]);
else
symmat2[j][i] = 0.0;
}
}
mat_free(evals);
mat_free(im);
return tmmps0;
}
void EcefToLatLonAlt(MATRIX vector)
{
int i;
double x, y, z, q, p, sinlat, sinlat2;
double a, b, d, radius, lat, alt, dummy;
double E_WGS84, E2_WGS84, ONE_MIN_E2, A_WGS84;
MATRIX lla;
lla = mat_creat(3,1,ZERO_MATRIX);
E_WGS84 = ECCENTRICITY; /* Earth ellipse ecc - unitless */
E2_WGS84 = E_WGS84*E_WGS84; /* Earth's ellipse ecc^2 - unitless */
ONE_MIN_E2 = 1.0 - E2_WGS84;
A_WGS84 = EARTH_RADIUS; /* Earth's ellipse semi-major axis - meters */
x = vector[0][0];
y = vector[1][0];
z = vector[2][0];
lla[1][0] = atan2(y, x); /* Longitude */
p = sqrt((x * x) + (y * y)); /* Latitude and Altitude */
if (p < 0.1)
{
p = 0.1;
}
q = z / p;
alt = 0.0;
lat = atan(q * (1.0 / ONE_MIN_E2));
a = 1.0;
i = 0;
while ((a > 0.2) && (i < 20))
{
sinlat = sin(lat);
sinlat2 = sinlat * sinlat;
dummy =sqrt((1.0 - (E2_WGS84 * sinlat2))*(1.0 - (E2_WGS84 * sinlat2)));
radius = A_WGS84 / sqrt(dummy);
d = alt;
alt = (p / cos(lat)) - radius;
a = q * (radius + alt);
b = (ONE_MIN_E2 * radius) + alt;
lat = atan2(a, b);
a = sqrt((alt - d)*(alt - d));
i = i + 1;
}
lla[0][0] = lat;
lla[2][0] = alt;
for (i = 0; i < 3; i++)
{
vector[i][0] = lla[i][0];
}
mat_free(lla);
}
void init_nav(struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){
/*++++++++++++++++++++++++++++++++++++++++++++++++
*matrix creation for navigation computation
*++++++++++++++++++++++++++++++++++++++++++++++++*/
F = mat_creat(15,15,ZERO_MATRIX); // State Matrix
PHI = mat_creat(15,15,ZERO_MATRIX); // State transition Matrix
P = mat_creat(15,15,ZERO_MATRIX); // Covariance Matrix
G = mat_creat(15,12,ZERO_MATRIX); // for Process Noise Transformation
Rw = mat_creat(12,12,ZERO_MATRIX);
Qw = mat_creat(15,15,ZERO_MATRIX);
Q = mat_creat(15,15,ZERO_MATRIX); // Process Noise Matrix
R = mat_creat(6,6,ZERO_MATRIX); // GPS Measurement Noise matrix
x = mat_creat(15,1,ZERO_MATRIX);
y = mat_creat(6,1,ZERO_MATRIX); // GPS Measurement
eul = mat_creat(3,1,ZERO_MATRIX);
Rbodtonav = mat_creat(3,3,ZERO_MATRIX);
grav = mat_creat(3,1,ZERO_MATRIX); // Gravity Model
f_b = mat_creat(3,1,ZERO_MATRIX);
om_ib = mat_creat(3,1,ZERO_MATRIX);
nr = mat_creat(3,1,ZERO_MATRIX); // Nav Transport Rate
K = mat_creat(15,6,ZERO_MATRIX); // Kalman Gain
H = mat_creat(6,15,ZERO_MATRIX);
C_N2B = mat_creat(3,3,ZERO_MATRIX); // DCM
C_B2N = mat_creat(3,3,ZERO_MATRIX); // DCM Transpose
pos_ref = mat_creat(3,1,ZERO_MATRIX);
pos_ins_ecef= mat_creat(3,1,ZERO_MATRIX);
pos_ins_ned = mat_creat(3,1,ZERO_MATRIX);
pos_gps = mat_creat(3,1,ZERO_MATRIX);
pos_gps_ecef= mat_creat(3,1,ZERO_MATRIX);
pos_gps_ned = mat_creat(3,1,ZERO_MATRIX);
I15 = mat_creat(15,15,UNIT_MATRIX); // Identity
I3 = mat_creat(3,3,UNIT_MATRIX); // Identity
ImKH = mat_creat(15,15,ZERO_MATRIX);
KRKt = mat_creat(15,15,ZERO_MATRIX);
dx = mat_creat(3,1,ZERO_MATRIX); // Temporary to get dxdt
a_temp31 = mat_creat(3,1,ZERO_MATRIX); // Temporary
b_temp31 = mat_creat(3,1,ZERO_MATRIX); // Temporary
temp33 = mat_creat(3,3,ZERO_MATRIX); // Temporary
atemp33 = mat_creat(3,3,ZERO_MATRIX); // Temporary
temp1515 = mat_creat(15,15,ZERO_MATRIX);
temp615 = mat_creat(6,15,ZERO_MATRIX);
temp66 = mat_creat(6,6,ZERO_MATRIX);
atemp66 = mat_creat(6,6,ZERO_MATRIX);
temp156 = mat_creat(15,6,ZERO_MATRIX);
temp1512 = mat_creat(15,12,ZERO_MATRIX);
// Assemble the matrices
// .... gravity, g
grav[2][0] = g;
// ... H
H[0][0] = 1.0; H[1][1] = 1.0; H[2][2] = 1.0;
H[3][3] = 1.0; H[4][4] = 1.0; H[5][5] = 1.0;
// first order correlation + white noise, tau = time constant for correlation
// gain on white noise plus gain on correlation
// Rw small - trust time update, Rw more - lean on measurement update
// split between accels and gyros and / or noise and correlation
// ... Rw
Rw[0][0] = SIG_W_AX*SIG_W_AX; Rw[1][1] = SIG_W_AY*SIG_W_AY; Rw[2][2] = SIG_W_AZ*SIG_W_AZ; //1 sigma on noise
Rw[3][3] = SIG_W_GX*SIG_W_GX; Rw[4][4] = SIG_W_GY*SIG_W_GY; Rw[5][5] = SIG_W_GZ*SIG_W_GZ;
Rw[6][6] = 2*SIG_A_D*SIG_A_D/TAU_A; Rw[7][7] = 2*SIG_A_D*SIG_A_D/TAU_A; Rw[8][8] = 2*SIG_A_D*SIG_A_D/TAU_A;
Rw[9][9] = 2*SIG_G_D*SIG_G_D/TAU_G; Rw[10][10] = 2*SIG_G_D*SIG_G_D/TAU_G; Rw[11][11] = 2*SIG_G_D*SIG_G_D/TAU_G;
// ... P (initial)
P[0][0] = P_P_INIT*P_P_INIT; P[1][1] = P_P_INIT*P_P_INIT; P[2][2] = P_P_INIT*P_P_INIT;
P[3][3] = P_V_INIT*P_V_INIT; P[4][4] = P_V_INIT*P_V_INIT; P[5][5] = P_V_INIT*P_V_INIT;
P[6][6] = P_A_INIT*P_A_INIT; P[7][7] = P_A_INIT*P_A_INIT; P[8][8] = P_HDG_INIT*P_HDG_INIT;
P[9][9] = P_AB_INIT*P_AB_INIT; P[10][10] = P_AB_INIT*P_AB_INIT; P[11][11] = P_AB_INIT*P_AB_INIT;
P[12][12] = P_GB_INIT*P_GB_INIT; P[13][13] = P_GB_INIT*P_GB_INIT; P[14][14] = P_GB_INIT*P_GB_INIT;
// ... update P in get_nav
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];
// ... R
R[0][0] = SIG_GPS_P_NE*SIG_GPS_P_NE; R[1][1] = SIG_GPS_P_NE*SIG_GPS_P_NE; R[2][2] = SIG_GPS_P_D*SIG_GPS_P_D;
R[3][3] = SIG_GPS_V*SIG_GPS_V; R[4][4] = SIG_GPS_V*SIG_GPS_V; R[5][5] = SIG_GPS_V*SIG_GPS_V;
// .. then initialize states with GPS Data
navData_ptr->lat = sensorData_ptr->gpsData_ptr->lat*D2R;
navData_ptr->lon = sensorData_ptr->gpsData_ptr->lon*D2R;
navData_ptr->alt = sensorData_ptr->gpsData_ptr->alt;
navData_ptr->vn = sensorData_ptr->gpsData_ptr->vn;
navData_ptr->ve = sensorData_ptr->gpsData_ptr->ve;
navData_ptr->vd = sensorData_ptr->gpsData_ptr->vd;
//navData_ptr->the = 0;//asin(sensorData_ptr->imuData_ptr->ax/g); // theta from Ax, aircraft at rest
//navData_ptr->phi = 0;//asin(-sensorData_ptr->imuData_ptr->ay/(g*cos(navData_ptr->the))); // phi from Ay, aircraft at rest
//navData_ptr->psi = 0.0;
// ... and initialize states with IMU Data
navData_ptr->the = asin(sensorData_ptr->imuData_ptr->ax/g); // theta from Ax, aircraft at rest
navData_ptr->phi = asin(-sensorData_ptr->imuData_ptr->ay/(g*cos(navData_ptr->the))); // phi from Ay, aircraft at rest
if((sensorData_ptr->imuData_ptr->hy) > 0){
navData_ptr->psi = 90*D2R - atan(sensorData_ptr->imuData_ptr->hx / sensorData_ptr->imuData_ptr->hy);
}
else if((sensorData_ptr->imuData_ptr->hy) < 0){
navData_ptr->psi = 270*D2R - atan(sensorData_ptr->imuData_ptr->hx / sensorData_ptr->imuData_ptr->hy) - 360*D2R;
}
else if((sensorData_ptr->imuData_ptr->hy) == 0){
if((sensorData_ptr->imuData_ptr->hx) < 0){
navData_ptr->psi = 180*D2R;
}
else{
navData_ptr->psi = 0.0;
}
}
eul2quat(navData_ptr->quat,(navData_ptr->phi),(navData_ptr->the),(navData_ptr->psi));
navData_ptr->ab[0] = 0.0;
navData_ptr->ab[1] = 0.0;
navData_ptr->ab[2] = 0.0;
navData_ptr->gb[0] = sensorData_ptr->imuData_ptr->p;
navData_ptr->gb[1] = sensorData_ptr->imuData_ptr->q;
navData_ptr->gb[2] = sensorData_ptr->imuData_ptr->r;
// Specific forces and Rotation Rate
f_b[0][0] = sensorData_ptr->imuData_ptr->ax - navData_ptr->ab[0];
f_b[1][0] = sensorData_ptr->imuData_ptr->ay - navData_ptr->ab[1];
f_b[2][0] = sensorData_ptr->imuData_ptr->az - navData_ptr->ab[2];
om_ib[0][0] = sensorData_ptr->imuData_ptr->p - navData_ptr->gb[0];
om_ib[1][0] = sensorData_ptr->imuData_ptr->q - navData_ptr->gb[1];
om_ib[2][0] = sensorData_ptr->imuData_ptr->r - navData_ptr->gb[2];
// Time during initialization
tprev = sensorData_ptr->imuData_ptr->time;
navData_ptr->err_type = data_valid;
send_status("NAV filter initialized");
//fprintf(stderr,"NAV Data Initialization Completed\n");
}
/*
*-----------------------------------------------------------------------------
* funct: mat_inv
* desct: find inverse of a matrix
* given: a = square matrix a, and C, return for inv(a)
* retrn: square matrix Inverse(A), C
* NULL = fails, singular matrix
*-----------------------------------------------------------------------------
*/
MATRIX mat_inv( MATRIX a , MATRIX C)
{
MATRIX A, B, P;
int i, n;
#ifdef CONFORM_CHECK
if(MatCol(a)!=MatCol(C))
{
error("\nUnconformable matrices in routine mat_inv(): Col(A)!=Col(B)\n");
}
if(MatRow(a)!=MatRow(C))
{
error("\nUnconformable matrices in routine mat_inv(): Row(A)!=Row(B)\n");
}
#endif
n = MatCol(a);
A = mat_creat(MatRow(a), MatCol(a), UNDEFINED);
A = mat_copy(a, A);
B = mat_creat( n, 1, UNDEFINED );
P = mat_creat( n, 1, UNDEFINED );
/*
* - LU-decomposition -
* also check for singular matrix
*/
if (mat_lu(A, P) == -1)
{
mat_free(A);
mat_free(B);
mat_free(P);
return (NULL);
}
else
{
/* Bug??? was still mat_backsubs1 even when singular??? */
for (i=0; i<n; i++)
{
mat_fill(B, ZERO_MATRIX);
B[i][0] = 1.0;
mat_backsubs1( A, B, C, P, i );
}
mat_free(A);
mat_free(B);
mat_free(P);
return (C);
}
}
