void kalman2(const float *A, const float *Q, const float *R, float *x, const float *z, float *P) {
float x_apriori[2], P_apriori[4], At[4];
float K[4];
float tmp1[4], tmp2[4];
// x_apriori = A*x
mat_mul(A, x, x_apriori, 2, 2, 1);
// P_apriori = A*P_aposteriori*At + Q
mat_mul(A, P, tmp1, 2, 2, 2);
transpose(A, At, 2, 2);
mat_mul(tmp1, At, tmp2, 2, 2, 2);
mat_add(tmp2, Q, P_apriori, 2, 2);
// K = P_apriori(P_apriori + R)^-1
mat_add(P_apriori, R, tmp1, 2, 2);
mat2_inv(tmp1, tmp2);
mat_mul(P_apriori, tmp2, K, 2, 2, 2);
// x_aposteriori = x_apriori + K(z - x_apriori)
mat_sub(z, x_apriori, tmp1, 2, 1);
mat_mul(K, tmp1, tmp2, 2, 2, 1);
mat_add(x_apriori, tmp2, x, 2, 1);
// P_aposteriori = (I - K)P_apriori
mat_sub(I, K, tmp1, 2, 2);
mat_mul(tmp1, P_apriori, P, 2, 2, 2);
}
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
}
double second_subproblem_obj (double ** ytwo, double ** z, double ** wtwo, double rho, int N, double* lambda) {
double ** temp = mat_init (N, N);
double ** difftwo = mat_init (N, N);
mat_zeros (difftwo, N, N);
// reg = 0.5 * sum_k max_n | w_nk | -> group-lasso
mat_zeros (temp, N, N);
double * maxn = new double [N];
for (int i = 0; i < N; i ++) { // Ian: need initial
maxn[i] = -INF;
}
for (int i = 0; i < N; i ++) {
for (int j = 0; j < N; j ++) {
if (wtwo[i][j] > maxn[j])
maxn[j] = wtwo[i][j];
}
}
double sumk = 0.0;
for (int i = 0; i < N; i ++) {
sumk += lambda[i]*maxn[i];
}
double group_lasso = sumk;
// sum2 = y_2^T dot (w_2 - z) -> linear
mat_zeros (temp, N, N);
mat_sub (wtwo, z, difftwo, N, N);
mat_tdot (ytwo, difftwo, temp, N, N);
double sum2 = mat_sum (temp, N, N);
// sum3 = 0.5 * rho * || w_2 - z_2 ||^2 -> quadratic mat_zeros (temp, N, N);
mat_sub (wtwo, z, temp, N, N);
double sum3 = 0.5 * rho * mat_norm2 (temp, N, N);
mat_free (temp, N, N);
// ouput values of each components
#ifdef BLOCKWISE_DUMP
cout << "[Blockwise] (group_lasso, linear, quadratic) = ("
<< group_lasso << ", " << sum2 << ", " << sum3
<< ")" << endl;
#endif
//cerr << group_lasso << ", " << sum2 << ", " << sum3 << endl;
return group_lasso + sum2 + sum3;
}
int main(void)
{
Item x = 5, y = 2, x2 = 6, y2 = 3;
mat m1, m2, k1, k2;
int i, j;
printf("\nItems: ");
item_print(x);
printf(" ");
item_print(y);
printf("\n");
printf("Item Addition: ");
item_print(item_add(x, y));
printf("\n");
printf("Item Multiplication: ");
item_print(item_mul(x, y));
printf("\n");
printf("Item Substraction: ");
item_print(item_sub(x, y));
printf("\n");
printf("Item Division: ");
item_print(item_div(x, y));
printf("\n\n");
m1 = mat_create(3, 2);
m2 = mat_create(2, 3);
k1 = mat_create(2, 2);
k2 = mat_create(2, 2);
for (i = 0; i < mat_rows(m1); i++)
for (j = 0; j < mat_cols(m1); j++)
mat_add_elem(m1, i, j, x);
for (i = 0; i < mat_rows(m2); i++)
for (j = 0; j < mat_cols(m2); j++)
mat_add_elem(m2, i, j, y);
for (i = 0; i < mat_rows(k1); i++)
for (j = 0; j < mat_cols(k1); j++)
{
mat_add_elem(k1, i, j, x2);
mat_add_elem(k2, i, j, y2);
}
printf("k1 Matrix: \n");
mat_print(k1);
printf("k2 Matrix: \n");
mat_print(k2);
printf("Addition: \n");
mat_print(mat_add(k1, k2));
printf("Substraction: \n");
mat_print(mat_sub(k1, k2));
printf("m1 Matrix: \n");
mat_print(m1);
printf("m2 Matrix: \n");
mat_print(m2);
printf("Multiplication: \n");
mat_print(mat_mul(m1, m2));
return 0;
}
double first_subproblm_obj (double ** dist_mat, double ** yone, double ** zone, double ** wone, double rho, int N) {
double ** temp = mat_init (N, N);
double ** diffone = mat_init (N, N);
mat_zeros (diffone, N, N);
// sum1 = 0.5 * sum_n sum_k (w_nk * d^2_nk) -> loss
mat_zeros (temp, N, N);
mat_times (wone, dist_mat, temp, N, N);
double sum1 = 0.5 * mat_sum (temp, N, N);
// sum2 = y_1^T dot (w_1 - z) -> linear
mat_zeros (temp, N, N);
mat_sub (wone, zone, diffone, N, N); // temp = w_1 - z_1
mat_tdot (yone, diffone, temp, N, N);
double sum2 = mat_sum (temp, N, N);
// sum3 = 0.5 * rho * || w_1 - z_1 ||^2 -> quadratic
mat_zeros (temp, N, N);
mat_sub (wone, zone, temp, N, N);
double sum3 = 0.5 * rho * mat_norm2 (temp, N, N);
// sum4 = r dot (1 - sum_k w_nk) -> dummy
double * temp_vec = new double [N];
mat_sum_row (wone, temp_vec, N, N);
double dummy_penalty = 0.0;
for (int i = 0; i < N; i ++) {
dummy_penalty += r*(1 - temp_vec[i]);
}
double total = sum1+sum2+sum3+dummy_penalty;
#ifdef FRANK_WOLFE_DUMP
cout << "[Frank_wolfe] (loss, linear, quadratic, dummy, total) = ("
<< sum1 << ", " << sum2 << ", " << sum3 << ", " << dummy_penalty << ", " << total
<< ")" << endl;
#endif
mat_free (temp, N, N);
mat_free (diffone, N, N);
delete [] temp_vec;
return total;
}
void EcefToEnu(MATRIX outputVector, MATRIX inputVector, MATRIX position)
{
int i;
double lat, lon;
MATRIX C, ned, ref_position;
MATRIX position_copy, delta_pos;
C = mat_creat(3,3,ZERO_MATRIX);
ref_position = mat_creat(3,1,ZERO_MATRIX);
delta_pos = mat_creat(3,1,ZERO_MATRIX);
position_copy = mat_creat(MatRow(position),MatCol(position),ZERO_MATRIX);
mat_copy(position, position_copy);
lat = position[0][0];
lon = position[1][0];
LatLonAltToEcef(ref_position,position_copy);
mat_sub(inputVector,ref_position,delta_pos);
C[0][0] = -sin(lon);
C[0][1] = cos(lon);
C[0][2] = 0;
C[1][0] = -sin(lat)*cos(lon);
C[1][1] = -sin(lat)*sin(lon);
C[1][2] = cos(lat);
C[2][0] = cos(lat)*cos(lon);
C[2][1] = cos(lat)*sin(lon);
C[2][2] = sin(lat);
ned = mat_creat(MatRow(C),MatCol(delta_pos),ZERO_MATRIX);
mat_mul(C,delta_pos,ned);
for (i = 0; i < 3; i++)
{
outputVector[i][0] = ned[i][0];
}
mat_free(ned);
mat_free(C);
mat_free(delta_pos);
mat_free(ref_position);
mat_free(position_copy);
}
static double stress(mat z, mat dij, int* anchors, int power)
{
int i,j;
double sum;
mat d = some_pairs_dist(z,anchors,dij->r);
mat_sub(d,dij);
for(i = 0; i < d->r; i++) {
for(j = 0; j < d->c; j++) {
d->m[mindex(i,j,d)] = pow(d->m[mindex(i,j,d)],2);
if(dij->m[mindex(i,j,d)] != 0) {
d->m[mindex(i,j,d)] *= 1.0 / pow(dij->m[mindex(i,j,d)], power);
}
}
}
sum = mat_accu(d);
mat_free(d);
return sum;
}
void ekf_filter_update(struct ekf_filter* filter, double *y) {
/* update
E = H * P * H' + R
K = P * H' * inv(E)
P = P - K * H * P
X = X + K * err
*/
double err;
int n = filter->state_dim;
int m = filter->measure_dim;
filter->mfun(y, &err, filter->X, filter->H);
/* E = H * P * H' + R */
mat_mult(m, n, n, filter->tmp1, filter->H, filter->P);
mat_transpose(m, n, filter->tmp2, filter->H);
mat_mult(m, n, m, filter->tmp3, filter->tmp1, filter->tmp2);
mat_add(m, m, filter->E, filter->tmp3, filter->R);
/* K = P * H' * inv(E) */
mat_transpose(m, n, filter->tmp1, filter->H);
mat_mult(n, n, m, filter->tmp2, filter->P, filter->tmp1);
if (filter->measure_dim != 1) { printf("only dim 1 measure implemented for now\n"); exit(-1);}
mat_scal_mult(n, m, filter->K, 1./filter->E[0], filter->tmp2);
/* P = P - K * H * P */
mat_mult(n, m, n, filter->tmp1, filter->K, filter->H);
mat_mult(n, n, n, filter->tmp2, filter->tmp1, filter->P);
mat_sub(n, n, filter->P, filter->P, filter->tmp2);
/* X = X + err * K */
mat_add_scal_mult(n, m, filter->X, filter->X, err, filter->K);
}
void cvx_clustering ( double ** dist_mat, int fw_max_iter, int max_iter, int D, int N, double* lambda, double ** W) {
// parameters
double alpha = 0.1;
double rho = 1;
// iterative optimization
double error = INF;
double ** wone = mat_init (N, N);
double ** wtwo = mat_init (N, N);
double ** yone = mat_init (N, N);
double ** ytwo = mat_init (N, N);
double ** z = mat_init (N, N);
double ** diffzero = mat_init (N, N);
double ** diffone = mat_init (N, N);
double ** difftwo = mat_init (N, N);
mat_zeros (yone, N, N);
mat_zeros (ytwo, N, N);
mat_zeros (z, N, N);
mat_zeros (diffzero, N, N);
mat_zeros (diffone, N, N);
mat_zeros (difftwo, N, N);
int iter = 0; // Ian: usually we count up (instead of count down)
while ( iter < max_iter ) { // stopping criteria
#ifdef SPARSE_CLUSTERING_DUMP
cout << "it is place 0 iteration #" << iter << ", going to get into frank_wolfe" << endl;
#endif
// mat_set (wone, z, N, N);
// mat_set (wtwo, z, N, N);
// mat_zeros (wone, N, N);
// mat_zeros (wtwo, N, N);
// STEP ONE: resolve w_1 and w_2
frank_wolf (dist_mat, yone, z, wone, rho, N, fw_max_iter); // for w_1
#ifdef SPARSE_CLUSTERING_DUMP
cout << "norm2(w_1) = " << mat_norm2 (wone, N, N) << endl;
#endif
blockwise_closed_form (ytwo, z, wtwo, rho, lambda, N); // for w_2
#ifdef SPARSE_CLUSTERING_DUMP
cout << "norm2(w_2) = " << mat_norm2 (wtwo, N, N) << endl;
#endif
// STEP TWO: update z by averaging w_1 and w_2
mat_add (wone, wtwo, z, N, N);
mat_dot (0.5, z, z, N, N);
#ifdef SPARSE_CLUSTERING_DUMP
cout << "norm2(z) = " << mat_norm2 (z, N, N) << endl;
#endif
// STEP THREE: update the y_1 and y_2 by w_1, w_2 and z
mat_sub (wone, z, diffone, N, N);
double trace_wone_minus_z = mat_norm2 (diffone, N, N);
mat_dot (alpha, diffone, diffone, N, N);
mat_add (yone, diffone, yone, N, N);
mat_sub (wtwo, z, difftwo, N, N);
//double trace_wtwo_minus_z = mat_norm2 (difftwo, N, N);
mat_dot (alpha, difftwo, difftwo, N, N);
mat_add (ytwo, difftwo, ytwo, N, N);
// STEP FOUR: trace the objective function
if (iter % 100 == 0) {
error = overall_objective (dist_mat, lambda, N, z);
// get current number of employed centroid
#ifdef NCENTROID_DUMP
int nCentroids = get_nCentroids (z, N, N);
#endif
cout << "[Overall] iter = " << iter
<< ", Overall Error: " << error
#ifdef NCENTROID_DUMP
<< ", nCentroids: " << nCentroids
#endif
<< endl;
}
iter ++;
}
// STEP FIVE: memory recollection
mat_free (wone, N, N);
mat_free (wtwo, N, N);
mat_free (yone, N, N);
mat_free (ytwo, N, N);
mat_free (diffone, N, N);
mat_free (difftwo, N, N);
// STEP SIX: put converged solution to destination W
mat_copy (z, W, N, N);
}
void blockwise_closed_form (double ** ytwo, double ** ztwo, double ** wtwo, double rho, double* lambda, int N) {
// STEP ONE: compute the optimal solution for truncated problem
double ** wbar = mat_init (N, N);
mat_zeros (wbar, N, N);
mat_dot (rho, ztwo, wbar, N, N); // wbar = rho * z_2
mat_sub (wbar, ytwo, wbar, N, N); // wbar = rho * z_2 - y_2
mat_dot (1.0/rho, wbar, wbar, N, N); // wbar = (rho * z_2 - y_2) / rho
// STEP TWO: find the closed-form solution for second subproblem
for (int j = 0; j < N; j ++) {
// 1. bifurcate the set of values
vector< pair<int,double> > alpha_vec;
for (int i = 0; i < N; i ++) {
double value = wbar[i][j];
/*if( wbar[i][j] < 0 ){
cerr << "wbar[" << i << "][" << j << "]" << endl;
exit(0);
}*/
alpha_vec.push_back (make_pair(i, abs(value)));
}
// 2. sorting
std::sort (alpha_vec.begin(), alpha_vec.end(), pairComparator);
/*
for (int i = 0; i < N; i ++) {
if (alpha_vec[i].second != 0)
cout << alpha_vec[i].second << endl;
}
*/
// 3. find mstar
int mstar = 0; // number of elements support the sky
double separator;
double max_term = -INF, new_term;
double sum_alpha = 0.0;
for (int i = 0; i < N; i ++) {
sum_alpha += alpha_vec[i].second;
new_term = (sum_alpha - lambda[j]) / (i + 1.0);
if ( new_term > max_term ) {
separator = alpha_vec[i].second;
max_term = new_term;
mstar = i;
}
}
// 4. assign closed-form solution to wtwo
if( max_term < 0 ) {
for(int i=0; i<N; i++)
wtwo[i][j] = 0.0;
continue;
}
for (int i = 0; i < N; i ++) {
// harness vector of pair
double value = wbar[i][j];
if ( abs(value) >= separator ) {
wtwo[i][j] = max_term;
} else {
// its ranking is above m*, directly inherit the wbar
wtwo[i][j] = max(wbar[i][j],0.0);
}
}
}
// compute value of objective function
double penalty = second_subproblem_obj (ytwo, ztwo, wtwo, rho, N, lambda);
// report the #iter and objective function
/*cout << "[Blockwise] second_subproblem_obj: " << penalty << endl;
cout << endl;*/
// STEP THREE: recollect temporary variable - wbar
mat_free (wbar, N, N);
}
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);
}
}
void expm(Matrix A, Matrix B, double h) {
static int firsttime = TRUE;
static Matrix M;
static Matrix M2;
static Matrix M3;
static Matrix D;
static Matrix N;
if (firsttime) {
firsttime = FALSE;
M = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
M2 = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
M3 = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
D = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
N = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
}
int norm = 0;
double c = 0.5;
int q = 6; // uneven p-q order for Pade approx.
int p = 1;
int i,j,k;
// Form the big matrix
mat_mult_scalar(A, h, A);
mat_equal_size(A, 2*N_DOFS, 2*N_DOFS, M);
for (i = 1; i <= 2*N_DOFS; ++i) {
for (j = 1; j <= N_DOFS; ++j) {
M[i][2*N_DOFS + j] = h * B[i][j];
}
}
//print_mat("M is:\n", M);
// scale M by power of 2 so that its norm < 1/2
norm = (int)(log2(inf_norm(M))) + 2;
//printf("Inf norm of M: %f \n", inf_norm(M));
if (norm < 0)
norm = 0;
mat_mult_scalar(M, 1/pow(2,(double)norm), M);
//printf("Norm of M logged, floored and 2 added is: %d\n", norm);
//print_mat("M after scaling is:\n", M);
for (i = 1; i <= 3*N_DOFS; i++) {
for (j = 1; j <= 3*N_DOFS; j++) {
N[i][j] = c * M[i][j];
D[i][j] = -c * M[i][j];
}
N[i][i] = N[i][i] + 1.0;
D[i][i] = D[i][i] + 1.0;
}
// set M2 equal to M
mat_equal(M, M2);
// start pade approximation
for (k = 2; k <= q; k++) {
c = c * (q - k + 1) / (double)(k * (2*q - k + 1));
mat_mult(M,M2,M2);
mat_mult_scalar(M2,c,M3);
mat_add(N,M3,N);
if (p == 1)
mat_add(D,M3,D);
else
mat_sub(D,M3,D);
p = -1 * p;
}
// multiply denominator with nominator i.e. D\E
my_inv_ludcmp(D, 3*N_DOFS, D);
mat_mult(D,N,N);
// undo scaling by repeated squaring
for (k = 1; k <= norm; k++)
mat_mult(N,N,N);
// get the matrices out
// read off the entries
for (i = 1; i <= 2*N_DOFS; i++) {
for (j = 1; j <= 2*N_DOFS; j++) {
A[i][j] = N[i][j];
}
for (j = 1; j <= N_DOFS; j++) {
B[i][j] = N[i][2*N_DOFS + j];
}
}
}
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;
}
// 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;
}
/**
* ICA function. Computes the W matrix from the
* preprocessed data.
*/
static mat ICA_compute(mat X, int rows, int cols)
{
mat TXp, GWX, W, Wd, W1, D, TU, TMP;
vect d, lim;
int i, it;
// matrix creation
TXp = mat_create(cols, rows);
GWX = mat_create(rows, cols);
W = mat_create(rows, rows);
Wd = mat_create(rows, rows);
D = mat_create(rows, rows);
TMP = mat_create(rows, rows);
TU = mat_create(rows, rows);
W1 = mat_create(rows, rows);
d = vect_create(rows);
// W rand init
mat_apply_fx(W, rows, rows, fx_rand, 0);
// sW <- La.svd(W)
mat_copy(W, rows, rows, Wd);
svdcmp(Wd, rows, rows, d, D);
// W <- sW$u %*% diag(1/sW$d) %*% t(sW$u) %*% W
mat_transpose(Wd, rows, rows, TU);
vect_apply_fx(d, rows, fx_inv, 0);
mat_diag(d, rows, D);
mat_mult(Wd, rows, rows, D, rows, rows, TMP);
mat_mult(TMP, rows, rows, TU, rows, rows, D);
mat_mult(D, rows, rows, W, rows, rows, Wd); // W = Wd
// W1 <- W
mat_copy(Wd, rows, rows, W1);
// lim <- rep(1000, maxit); it = 1
lim = vect_create(MAX_ITERATIONS);
for (i=0; i<MAX_ITERATIONS; i++)
lim[i] = 1000;
it = 0;
// t(X)/p
mat_transpose(X, rows, cols, TXp);
mat_apply_fx(TXp, cols, rows, fx_div_c, cols);
while (lim[it] > TOLERANCE && it < MAX_ITERATIONS) {
// wx <- W %*% X
mat_mult(Wd, rows, rows, X, rows, cols, GWX);
// gwx <- tanh(alpha * wx)
mat_apply_fx(GWX, rows, cols, fx_tanh, 0);
// v1 <- gwx %*% t(X)/p
mat_mult(GWX, rows, cols, TXp, cols, rows, TMP); // V1 = TMP
// g.wx <- alpha * (1 - (gwx)^2)
mat_apply_fx(GWX, rows, cols, fx_1sub_sqr, 0);
// v2 <- diag(apply(g.wx, 1, FUN = mean)) %*% W
mat_mean_rows(GWX, rows, cols, d);
mat_diag(d, rows, D);
mat_mult(D, rows, rows, Wd, rows, rows, TU); // V2 = TU
// W1 <- v1 - v2
mat_sub(TMP, TU, rows, rows, W1);
// sW1 <- La.svd(W1)
mat_copy(W1, rows, rows, W);
svdcmp(W, rows, rows, d, D);
// W1 <- sW1$u %*% diag(1/sW1$d) %*% t(sW1$u) %*% W1
mat_transpose(W, rows, rows, TU);
vect_apply_fx(d, rows, fx_inv, 0);
mat_diag(d, rows, D);
mat_mult(W, rows, rows, D, rows, rows, TMP);
mat_mult(TMP, rows, rows, TU, rows, rows, D);
mat_mult(D, rows, rows, W1, rows, rows, W); // W1 = W
// lim[it + 1] <- max(Mod(Mod(diag(W1 %*% t(W))) - 1))
mat_transpose(Wd, rows, rows, TU);
mat_mult(W, rows, rows, TU, rows, rows, TMP);
lim[it+1] = fabs(mat_max_diag(TMP, rows, rows) - 1);
// W <- W1
mat_copy(W, rows, rows, Wd);
it++;
}
// clean up
mat_delete(TXp, cols, rows);
mat_delete(GWX, rows, cols);
mat_delete(W, rows, rows);
mat_delete(D, rows, rows);
mat_delete(TMP, rows, rows);
mat_delete(TU, rows, rows);
mat_delete(W1, rows, rows);
vect_delete(d);
return Wd;
}
/*!*****************************************************************************
*******************************************************************************
\note compute_kinematics
\date June 99
\remarks
computes kinematic variables
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
static void
compute_kinematics(void)
{
int i,j,r,o;
static int firsttime = TRUE;
static SL_DJstate *temp;
static Matrix last_J;
static Matrix last_Jbase;
if (firsttime) {
temp=(SL_DJstate *)my_calloc((unsigned long)(n_dofs+1),sizeof(SL_DJstate),MY_STOP);
last_J = my_matrix(1,6*n_endeffs,1,n_dofs);
last_Jbase = my_matrix(1,6*n_endeffs,1,2*N_CART);
}
/* compute the desired link positions */
linkInformationDes(joint_des_state,&base_state,&base_orient,endeff,
joint_cog_mpos_des,joint_axis_pos_des,joint_origin_pos_des,
link_pos_des,Alink_des,Adof_des);
/* the desired endeffector information */
for (i=1; i<=N_CART; ++i) {
for (j=1; j<=n_endeffs; ++j) {
cart_des_state[j].x[i] = link_pos_des[link2endeffmap[j]][i];
}
}
/* the desired quaternian of the endeffector */
for (j=1; j<=n_endeffs; ++j) {
linkQuat(Alink_des[link2endeffmap[j]],&(cart_des_orient[j]));
}
/* addititional link information */
linkInformation(joint_state,&base_state,&base_orient,endeff,
joint_cog_mpos,joint_axis_pos,joint_origin_pos,
link_pos,Alink,Adof);
/* create the endeffector information */
for (i=1; i<=N_CART; ++i) {
for (j=1; j<=n_endeffs; ++j) {
cart_state[j].x[i] = link_pos[link2endeffmap[j]][i];
}
}
/* the quaternian of the endeffector */
for (j=1; j<=n_endeffs; ++j) {
linkQuat(Alink[link2endeffmap[j]],&(cart_orient[j]));
}
/* the COG position */
compute_cog();
/* the jacobian */
jacobian(link_pos,joint_origin_pos,joint_axis_pos,J);
baseJacobian(link_pos,joint_origin_pos,joint_axis_pos,Jbase);
jacobian(link_pos_des,joint_origin_pos_des,joint_axis_pos_des,Jdes);
baseJacobian(link_pos_des,joint_origin_pos_des,joint_axis_pos_des,Jbasedes);
/* numerical time derivative of Jacobian */
if (!firsttime) {
mat_sub(J,last_J,dJdt);
mat_mult_scalar(dJdt,(double)ros_servo_rate,dJdt);
mat_sub(Jbase,last_Jbase,dJbasedt);
mat_mult_scalar(dJbasedt,(double)ros_servo_rate,dJbasedt);
}
mat_equal(J,last_J);
mat_equal(Jbase,last_Jbase);
/* compute the cartesian velocities and accelerations */
for (j=1; j<=n_endeffs; ++j) {
for (i=1; i<=N_CART; ++i) {
cart_state[j].xd[i] = 0.0;
cart_state[j].xdd[i] = 0.0;
cart_orient[j].ad[i] = 0.0;
cart_orient[j].add[i] = 0.0;
cart_des_state[j].xd[i] = 0.0;
cart_des_orient[j].ad[i] = 0.0;
/* contributations from the joints */
for (r=1; r<=n_dofs; ++r) {
cart_state[j].xd[i] += J[(j-1)*6+i][r] * joint_state[r].thd;
cart_orient[j].ad[i] += J[(j-1)*6+i+3][r] * joint_state[r].thd;
cart_des_state[j].xd[i] += Jdes[(j-1)*6+i][r] *joint_des_state[r].thd;
cart_des_orient[j].ad[i]+= Jdes[(j-1)*6+i+3][r] * joint_des_state[r].thd;
cart_state[j].xdd[i] += J[(j-1)*6+i][r] * joint_state[r].thdd +
dJdt[(j-1)*6+i][r] * joint_state[r].thd;
cart_orient[j].add[i] += J[(j-1)*6+i+3][r] * joint_state[r].thdd +
dJdt[(j-1)*6+i+3][r] * joint_state[r].thd;
}
/* contributations from the base */
for (r=1; r<=N_CART; ++r) {
cart_state[j].xd[i] += Jbase[(j-1)*6+i][r] * base_state.xd[r];
cart_orient[j].ad[i] += Jbase[(j-1)*6+i+3][r] * base_state.xd[r];
cart_state[j].xd[i] += Jbase[(j-1)*6+i][3+r] * base_orient.ad[r];
cart_orient[j].ad[i] += Jbase[(j-1)*6+i+3][3+r] * base_orient.ad[r];
cart_des_state[j].xd[i] += Jbasedes[(j-1)*6+i][r] * base_state.xd[r];
cart_des_orient[j].ad[i] += Jbasedes[(j-1)*6+i+3][r] * base_state.xd[r];
cart_des_state[j].xd[i] += Jbasedes[(j-1)*6+i][3+r] * base_orient.ad[r];
cart_des_orient[j].ad[i] += Jbasedes[(j-1)*6+i+3][3+r] * base_orient.ad[r];
cart_state[j].xdd[i] += Jbase[(j-1)*6+i][r] * base_state.xdd[r] +
dJbasedt[(j-1)*6+i][r] * base_state.xd[r];
cart_orient[j].add[i] += Jbase[(j-1)*6+i+3][r] * base_state.xdd[r] +
dJbasedt[(j-1)*6+i+3][r] * base_state.xd[r];
cart_state[j].xdd[i] += Jbase[(j-1)*6+i][3+r] * base_orient.add[r] +
dJbasedt[(j-1)*6+i][3+r] * base_orient.ad[r];
cart_orient[j].add[i] += Jbase[(j-1)*6+i+3][3+r] * base_orient.add[r] +
dJbasedt[(j-1)*6+i+3][3+r] * base_orient.ad[r];
}
}
/* compute quaternion derivatives */
quatDerivatives(&(cart_orient[j]));
quatDerivatives(&(cart_des_orient[j]));
for (r=1; r<=N_QUAT; ++r)
cart_des_orient[j].qdd[r] = 0.0; // we don't have dJdes_dt so far
}
/* reset first time flag */
firsttime = FALSE;
}
int main()
{
unsigned r1, c1, r2, c2, ch;
mat m1, m2, t;
puts("Enter row and column numbers of matrices 1 and 2:");
scanf(" %u %u %u %u%*c", &r1, &c1, &r2, &c2);
putchar('\n');
m1=mat_alloc(r1, c1);
m2=mat_alloc(r2, c2);
printf("Enter value of Matrix 1 (%ux%u):\n", r1, c1);
mat_read(m1);
putchar('\n');
printf("Enter value of Matrix 2 (%ux%u):\n", r2, c2);
mat_read(m2);
putchar('\n');
do{
puts("What would you like to do?");
puts(" ( 0) Exit");
puts(" ( 1) Display");
puts(" ( 2) Transpose");
puts(" ( 3) Sum");
puts(" ( 4) Difference");
puts(" ( 5) Product");
puts(" ( 6) Greatest Element of Rows");
puts(" ( 7) Sum of Major Diagonal");
puts(" ( 8) Sum of Minor Diagonal");
puts(" ( 9) Check Symmetricity");
puts(" (10) Upper-Triangular Matrix");
puts(" (11) Lower-Triangular Matrix");
scanf(" %u%*c", &ch);
switch(ch){
case 0:
puts("Bye!");
break;
case 1:
puts("Matrix 1:");
mat_write(m1);
putchar('\n');
puts("Matrix 2:");
mat_write(m2);
break;
case 2:
t=mat_trn(m1);
mat_write(t);
mat_free(t);
break;
case 3:
if((t=mat_add(m1, m2)).r){
mat_write(t);
mat_free(t);
}
else puts("Not Possible");
break;
case 4:
if((t=mat_sub(m1, m2)).r){
mat_write(t);
mat_free(t);
}
else puts("Not Possible");
break;
case 5:
if((t=mat_mul(m1, m2)).r){
mat_write(t);
mat_free(t);
}
else puts("Not Possible");
break;
case 6:
row_great(m1);
break;
case 7:
add_major(m1);
break;
case 8:
add_minor(m1);
break;
case 9:
if(issymm(m1)) puts("Symmetric");
else puts("Unsymmetric");
break;
case 10:
up_tri(m1);
break;
case 11:
lo_tri(m1);
break;
default:
puts("Incorrect Choice!");
break;
}
putchar('\n');
} while(ch);
mat_free(m1);
mat_free(m2);
return 0;
}
/**
* ICA function. Computes the W matrix from the
* preprocessed data.
*/
static mat ICA_compute(mat X, int rows, int cols) {
mat TXp, GWX, W, Wd, W1, D, TU, TMP;
vect d, lim;
int i, it;
FILE *OutputFile;
clock_t clock1, clock2;
float time;
//char ascii_path[512];
//strcpy(ascii_path, "/storage/sdcard0/NickGun/EEG/Log.txt");
//FILE *Log;
// matrix creation
TXp = mat_create(cols, rows);
GWX = mat_create(rows, cols);
W = mat_create(rows, rows);
Wd = mat_create(rows, rows);
D = mat_create(rows, rows);
TMP = mat_create(rows, rows);
TU = mat_create(rows, rows);
W1 = mat_create(rows, rows);
d = vect_create(rows);
// W rand init
mat_apply_fx(W, rows, rows, fx_rand, 0);
// sW <- La.svd(W)
mat_copy(W, rows, rows, Wd);
svdcmp(Wd, rows, rows, d, D);
// W <- sW$u %*% diag(1/sW$d) %*% t(sW$u) %*% W
mat_transpose(Wd, rows, rows, TU);
vect_apply_fx(d, rows, fx_inv, 0);
mat_diag(d, rows, D);
mat_mult(Wd, rows, rows, D, rows, rows, TMP);
mat_mult(TMP, rows, rows, TU, rows, rows, D);
mat_mult(D, rows, rows, W, rows, rows, Wd); // W = Wd
// W1 <- W
mat_copy(Wd, rows, rows, W1);
// lim <- rep(1000, maxit); it = 1
lim = vect_create(MAX_ITERATIONS);
for (i = 0; i < MAX_ITERATIONS; i++)
lim[i] = 1000;
it = 0;
// t(X)/p
mat_transpose(X, rows, cols, TXp);
mat_apply_fx(TXp, cols, rows, fx_div_c, cols);
while (lim[it] > TOLERANCE && it < MAX_ITERATIONS) {
// wx <- W %*% X
mat_mult(Wd, rows, rows, X, rows, cols, GWX);
// gwx <- tanh(alpha * wx)
mat_apply_fx(GWX, rows, cols, fx_tanh, 0);
// v1 <- gwx %*% t(X)/p
mat_mult(GWX, rows, cols, TXp, cols, rows, TMP); // V1 = TMP
// g.wx <- alpha * (1 - (gwx)^2)
mat_apply_fx(GWX, rows, cols, fx_1sub_sqr, 0);
// v2 <- diag(apply(g.wx, 1, FUN = mean)) %*% W
mat_mean_rows(GWX, rows, cols, d);
mat_diag(d, rows, D);
mat_mult(D, rows, rows, Wd, rows, rows, TU); // V2 = TU
// W1 <- v1 - v2
mat_sub(TMP, TU, rows, rows, W1);
// sW1 <- La.svd(W1)
mat_copy(W1, rows, rows, W);
svdcmp(W, rows, rows, d, D);
// W1 <- sW1$u %*% diag(1/sW1$d) %*% t(sW1$u) %*% W1
mat_transpose(W, rows, rows, TU);
vect_apply_fx(d, rows, fx_inv, 0);
mat_diag(d, rows, D);
mat_mult(W, rows, rows, D, rows, rows, TMP);
mat_mult(TMP, rows, rows, TU, rows, rows, D);
mat_mult(D, rows, rows, W1, rows, rows, W); // W1 = W
// lim[it + 1] <- max(Mod(Mod(diag(W1 %*% t(W))) - 1))
mat_transpose(Wd, rows, rows, TU); //chuyen vi
mat_mult(W, rows, rows, TU, rows, rows, TMP); //TMP=WxTU
lim[it + 1] = fabs(mat_max_diag(TMP, rows, rows) - 1);
if(lim[it+1]<0.1)
break;
/*
OutputFile = fopen("/storage/sdcard0/Nickgun/EEG/data/lim",
"at");
fprintf(OutputFile, "%f \n", lim[it+1]);
fclose(OutputFile);
// W <- W1
*/
mat_copy(W, rows, rows, Wd);
it++;
}
// clean up
mat_delete(TXp, cols, rows);
mat_delete(GWX, rows, cols);
mat_delete(W, rows, rows);
mat_delete(D, rows, rows);
mat_delete(TMP, rows, rows);
mat_delete(TU, rows, rows);
mat_delete(W1, rows, rows);
vect_delete(d);
return Wd;
}