/**
* Implementation of the learning rule described in Bell & Sejnowski,
* Vision Research, in press for 1997, that contained the natural
* gradient (W' * W).
*
* Bell & Sejnowski hold the patent for this learning rule.
*
* SEP goes once through the mixed signals X in batch blocks of size B,
* adjusting weights W at the end of each block.
*
* sepout is called every F counts.
*
* I suggest a learning rate (L) of 0.006, and a block size (B) of
* 300, at least for 2->2 separation. When annealing to the right
* solution for 10->10, however, L < 0.0001 and B = 10 were most successful.
*
* @param X "sphered" input matrix
* @param W weight matrix
* @param B block size
* @param L learning rate
* @param F interval to print training stats
*/
void sep96(matrix_t *X, matrix_t *W, int B, double L, int F)
{
matrix_t *BI = m_identity(X->rows);
m_elem_mult(BI, B);
int t;
for ( t = 0; t < X->cols; t += B ) {
int end = (t + B < X->cols)
? t + B
: X->cols;
matrix_t *W0 = m_copy(W);
matrix_t *X_batch = m_copy_columns(X, t, end);
matrix_t *U = m_product(W0, X_batch);
// compute Y' = 1 - 2 * f(U), f(u) = 1 / (1 + e^(-u))
matrix_t *Y_p = m_initialize(U->rows, U->cols);
int i, j;
for ( i = 0; i < Y_p->rows; i++ ) {
for ( j = 0; j < Y_p->cols; j++ ) {
elem(Y_p, i, j) = 1 - 2 * (1 / (1 + exp(-elem(U, i, j))));
}
}
// compute dW = L * (BI + Y'U') * W0
matrix_t *U_tr = m_transpose(U);
matrix_t *dW_temp1 = m_product(Y_p, U_tr);
m_add(dW_temp1, BI);
matrix_t *dW = m_product(dW_temp1, W0);
m_elem_mult(dW, L);
// compute W = W0 + dW
m_add(W, dW);
// print training stats
if ( t % F == 0 ) {
precision_t norm = m_norm(dW);
precision_t angle = m_angle(W0, W);
printf("*** norm(dW) = %.4lf, angle(W0, W) = %.1lf deg\n", norm, 180 * angle / M_PI);
}
// cleanup
m_free(W0);
m_free(X_batch);
m_free(U);
m_free(Y_p);
m_free(U_tr);
m_free(dW_temp1);
m_free(dW);
}
}
int main(int argc, char *argv[]) {
vect3<TYP> r, s, t;
cout << "\n\nZadej 1. vektor (r): ";
cin >> r;
cout << "\nr = " << r << ", jeho delka = " << abs(r) << "\n"
<< "Opacny vektor = " << -r << ", jeho delka = " << abs(-r)
<< "\n-r/2 = " << static_cast<TYP>(-0.5)*r
<< "\nDelka polovicniho opacneho = "
<< abs(static_cast<TYP>(-0.5)*r)
<< "\n\nZadej 2. vektor (s): ";
cin >> s;
cout << "\ns = " << s << "\nr + s = " << r+s << "\nr - s = "
<< r-s << "\nr . s = " << r*s << "\nr x s = " << r%s
<< "\ns . r = " << s*r << "\ns x r = " << s%r
<< " (opacny k r x s)\n\nZadej 3. vektor (t): ";
cin >> t;
cout << "\nt = " << t << "\nSmiseny soucin = " << m_product(r,s,t)
<< "\n\n";
return 0;
}
/**
* Test matrix square root.
*/
void test_m_sqrtm()
{
precision_t data[][5] = {
{ 5, -4, 1, 0, 0 },
{ -4, 6, -4, 1, 0 },
{ 1, -4, 6, -4, 1 },
{ 0, 1, -4, 6, -4 },
{ 0, 0, 1, -4, 6 }
};
matrix_t *A = m_initialize(5, 5);
fill_matrix_data(A, data);
matrix_t *X = m_sqrtm(A);
matrix_t *X_sq = m_product(X, X);
printf("A = \n");
m_fprint(stdout, A);
printf("X = sqrtm(A) = \n");
m_fprint(stdout, X);
printf("X * X = \n");
m_fprint(stdout, X_sq);
m_free(A);
m_free(X);
m_free(X_sq);
}
/**
* Test matrix inverse.
*/
void test_m_inverse()
{
precision_t data[][3] = {
{ 1, 0, 2 },
{ -1, 5, 0 },
{ 0, 3, -9 }
};
matrix_t *X = m_initialize(3, 3);
fill_matrix_data(X, data);
matrix_t *Y = m_inverse(X);
matrix_t *Z = m_product(Y, X);
printf("X = \n");
m_fprint(stdout, X);
printf("Y = inv(X) = \n");
m_fprint(stdout, Y);
printf("Y * X = \n");
m_fprint(stdout, Z);
m_free(X);
m_free(Y);
m_free(Z);
}
/**
* Compute the projection matrix of a training set with ICA
* using Architecture II.
*
* @param W_pca_tr PCA projection matrix
* @param P_pca PCA projected images
* @return projection matrix W_ica'
*/
matrix_t * ICA2(matrix_t *W_pca_tr, matrix_t *P_pca)
{
// compute weight matrix W_I
matrix_t *W_I = run_ica(P_pca);
// compute W_ica' = W_I * W_pca'
matrix_t *W_ica_tr = m_product(W_I, W_pca_tr);
// cleanup
m_free(W_I);
return W_ica_tr;
}
/**
* Compute the ICA weight matrix W_I for an input matrix X.
*
* @param X mean-subtracted input matrix
* @return weight matrix W_I
*/
matrix_t * run_ica(matrix_t *X)
{
// compute whitening matrix W_z
matrix_t *W_z = sphere(X);
matrix_t *X_sph = m_product(W_z, X);
// shuffle the columns of X_sph
m_shuffle_columns(X_sph);
// train the weight matrix W
matrix_t *W = m_identity(X->rows);
sep96_params_t params[] = {
{ 50, 0.0005, 5000, 1000 },
{ 50, 0.0003, 5000, 200 },
{ 50, 0.0002, 5000, 200 },
{ 50, 0.0001, 5000, 200 }
};
int num_sweeps = sizeof(params) / sizeof(sep96_params_t);
int i, j;
for ( i = 0; i < num_sweeps; i++ ) {
printf("sweep %d: B = %d, L = %lf\n", i + 1, params[i].B, params[i].L);
for ( j = 0; j < params[i].N; j++ ) {
sep96(X_sph, W, params[i].B, params[i].L, params[i].F);
}
}
// compute W_I = W * W_z
matrix_t *W_I = m_product(W, W_z);
// cleanup
m_free(W_z);
m_free(X_sph);
m_free(W);
return W_I;
}
void update_ahrs(float dt, float dcm_out[3][3], float gyr_out[3])
{
static uint8_t i;
read_mpu(v_gyr, v_acc);
for (i=0; i<3; i++) {
gyr_out[i] = v_gyr[i];
}
/**
* Accelerometer
* Frame of reference: body
* Units: G (gravitational acceleration)
* Purpose: Measure the acceleration vector v_acc with components
* codirectional with the i, j, and k vectors. Note that the
* gravitational vector is the negative of the K vector.
*/
#ifdef ACC_WEIGHT
// Take weighted average.
#ifdef ACC_SELF_WEIGHT
for (i=0; i<3; i++) {
v_acc[i] = ACC_SELF_WEIGHT * v_acc[i] + (1-ACC_SELF_WEIGHT) * v_acc_last[i];
v_acc_last[i] = v_acc[i];
// Kalman filtering?
//v_acc_last[i] = acc.get(i);
//kalmanUpdate(v_acc[i], accVar, v_acc_last[i], ACC_UPDATE_SIG);
//kalmanPredict(v_acc[i], accVar, 0.0, ACC_PREDICT_SIG);
}
#endif // ACC_SELF_WEIGHT
acc_scale = v_norm(v_acc);
/**
* Reduce accelerometer weight if the magnitude of the measured
* acceleration is significantly greater than or less than 1 g.
*
* TODO: Magnitude of acceleration should be reported over telemetry so
* ACC_SCALE_WEIGHT can be more accurately determined.
*/
#ifdef ACC_SCALE_WEIGHT
acc_scale = (1.0 - MIN(1.0, ACC_SCALE_WEIGHT * ABS(acc_scale - 1.0)));
acc_weight = ACC_WEIGHT * acc_scale;
#else
acc_weight = ACC_WEIGHT;
// Ignore accelerometer if it measures anything 0.5g past gravity.
if (acc_scale > 1.5 || acc_scale < 0.5) {
acc_weight = 0;
}
#endif // ACC_SCALE_WEIGHT
/**
* Express K global unit vector in body frame as k_gb for use in drift
* correction (we need K to be described in the body frame because gravity
* is measured by the accelerometer in the body frame). Technically we
* could just create a transpose of dcm_gyro, but since we don't (yet) have
* a magnetometer, we don't need the first two rows of the transpose. This
* saves a few clock cycles.
*/
for (i=0; i<3; i++) {
k_gb[i] = dcm_gyro[i][2];
}
/**
* Calculate gyro drift correction rotation vector w_a, which will be used
* later to bring KB closer to the gravity vector (i.e., the negative of
* the K vector). Although we do not explicitly negate the gravity vector,
* the cross product below produces a rotation vector that we can later add
* to the angular displacement vector to correct for gyro drift in the
* X and Y axes. Note we negate w_a because our acceleration vector is
* actually the negative of our gravity vector.
*/
v_crossp(k_gb, v_acc, w_a);
v_scale(w_a, -1, w_a);
#endif // ACC_WEIGHT
/**
* Magnetometer
* Frame of reference: body
* Units: N/A
* Purpose: Measure the magnetic north vector v_mag with components
* codirectional with the body's i, j, and k vectors.
*/
#ifdef MAG_WEIGHT
// Express J global unit vectory in body frame as j_gb.
for (i=0; i<3; i++) {
j_gb[i] = dcm_gyro[i][1];
}
// Calculate yaw drift correction vector w_m.
v_crossp(j_gb, v_mag, w_m);
#endif // MAG_WEIGHT
/**
* Gyroscope
* Frame of reference: body
* Units: rad/s
* Purpose: Measure the rotation rate of the body about the body's i,
* j, and k axes.
* ========================================================================
* Scale v_gyr by elapsed time (in seconds) to get angle w*dt in radians,
* then compute weighted average with the accelerometer and magnetometer
* correction vectors to obtain final w*dt.
* TODO: This is still not exactly correct.
*/
for (i=0; i<3; i++) {
w_dt[i] = v_gyr[i] * dt;
#ifdef ACC_WEIGHT
w_dt[i] += acc_weight * w_a[i];
#endif // ACC_WEIGHT
#ifdef MAG_WEIGHT
w_dt[i] += MAG_WEIGHT * w_m[i];
#endif // MAG_WEIGHT
}
/**
* Direction Cosine Matrix
* Frame of reference: global
* Units: None (unit vectors)
* Purpose: Calculate the components of the body's i, j, and k unit
* vectors in the global frame of reference.
* ========================================================================
* Skew the rotation vector and sum appropriate components by combining the
* skew symmetric matrix with the identity matrix. The math can be
* summarized as follows:
*
* All of this is calculated in the body frame. If w is the angular
* velocity vector, let w_dt (w*dt) be the angular displacement vector of
* the DCM over a time interval dt. Let w_dt_i, w_dt_j, and w_dt_k be the
* components of w_dt codirectional with the i, j, and k unit vectors,
* respectively. Also, let dr be the linear displacement vector of the DCM
* and dr_i, dr_j, and dr_k once again be the i, j, and k components,
* respectively.
*
* In very small dt, certain vectors approach orthogonality, so we can
* assume that (draw this out for yourself!):
*
* dr_x = < 0, dw_k, -dw_j>,
* dr_y = <-dw_k, 0, dw_i>, and
* dr_z = < dw_j, -dw_i, 0>,
*
* which can be expressed as the rotation matrix:
*
* [ 0 dw_k -dw_j ]
* dr = [ -dw_k 0 dw_i ]
* [ dw_j -dw_i 0 ].
*
* This can then be multiplied by the current DCM and added to the current
* DCM to update the DCM. To minimize the number of calculations performed
* by the processor, however, we can combine the last two steps by
* combining dr with the identity matrix to produce:
*
* [ 1 dw_k -dw_j ]
* dr + I = [ -dw_k 1 dw_i ]
* [ dw_j -dw_i 1 ],
*
* which we multiply with the current DCM to produce the updated DCM
* directly.
*
* It may be helpful to read the Wikipedia pages on cross products and
* rotation representation.
*/
dcm_d[0][0] = 1;
dcm_d[0][1] = w_dt[2];
dcm_d[0][2] = -w_dt[1];
dcm_d[1][0] = -w_dt[2];
dcm_d[1][1] = 1;
dcm_d[1][2] = w_dt[0];
dcm_d[2][0] = w_dt[1];
dcm_d[2][1] = -w_dt[0];
dcm_d[2][2] = 1;
// Multiply the current DCM with the change in DCM and update.
m_product(dcm_d, dcm_gyro, dcm_gyro);
// Remove any distortions introduced by the small-angle approximations.
orthonormalize(dcm_gyro);
// Apply rotational offset (in case IMU is not installed orthogonally to
// the airframe).
dcm_offset[0][0] = 1;
dcm_offset[0][1] = 0;
dcm_offset[0][2] = -trim_angle[1];
dcm_offset[1][0] = 0;
dcm_offset[1][1] = 1;
dcm_offset[1][2] = trim_angle[0];
dcm_offset[2][0] = trim_angle[1];
dcm_offset[2][1] = -trim_angle[0];
dcm_offset[2][2] = 1;
m_product(dcm_offset, dcm_gyro, dcm_out);
}
/**
* Test matrix product.
*/
void test_m_product()
{
// multiply two vectors, A * B
precision_t data_A1[][4] = {
{ 1, 1, 0, 0 }
};
precision_t data_B1[][1] = {
{ 1 },
{ 2 },
{ 3 },
{ 4 }
};
matrix_t *A = m_initialize(1, 4);
matrix_t *B = m_initialize(4, 1);
fill_matrix_data(A, data_A1);
fill_matrix_data(B, data_B1);
matrix_t *C = m_product(A, B);
printf("A = \n");
m_fprint(stdout, A);
printf("B = \n");
m_fprint(stdout, B);
printf("A * B = \n");
m_fprint(stdout, C);
m_free(C);
// multiply two vectors, B * A
C = m_product(B, A);
printf("B * A = \n");
m_fprint(stdout, C);
m_free(C);
m_free(A);
m_free(B);
// multiply two arrays
precision_t data_A2[][3] = {
{ 1, 3, 5 },
{ 2, 4, 7 }
};
precision_t data_B2[][3] = {
{ -5, 8, 11 },
{ 3, 9, 21 },
{ 4, 0, 8 }
};
A = m_initialize(2, 3);
B = m_initialize(3, 3);
fill_matrix_data(A, data_A2);
fill_matrix_data(B, data_B2);
C = m_product(A, B);
printf("A = \n");
m_fprint(stdout, A);
printf("B = \n");
m_fprint(stdout, B);
printf("A * B = \n");
m_fprint(stdout, C);
m_free(A);
m_free(B);
m_free(C);
}
