/**
* 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);
}
}
/*
* Kalman filter update
*
* P is [4,4] and holds the covariance matrix
* R is [3,3] and holds the measurement weights
* C is [4,3] and holds the euler to quat tranform
* X is [4,1] and holds the estimated state as a quaterion
* Xe is [3,1] and holds the euler angles of the estimated state
* Xm is [3,1] and holds the measured euler angles
*/
static inline void
kalman_update(
m_t P,
m_t R,
m_t C,
v_t X,
const v_t Xe,
const v_t Xm
)
{
m_t T1;
m_t T2;
m_t E;
m_t K;
v_t err;
v_t temp;
/* E[3,3] = C[3,4] P[4,4] C'[4,3] + R[3,3] */
m_mult( T1, C, P, 3, 4, 4 );
m_transpose( T2, C, 3, 4 );
m_mult( E, T1, T2, 3, 4, 3 );
m_add( E, R, E, 3, 3 );
/* K[4,3] = P[4,4] C'[4,3] inv(E)[3,3] */
m_mult( T1, P, T2, 4, 4, 3 );
m33_inv( E, T2 );
m_mult( K, T1, T2, 4, 3, 3 );
/* X[4] = X[4] + K[4,3] ( Xm[3] - Xe[3] ) */
v_sub( err, Xm, Xe, 3 );
mv_mult( temp, K, err, 4, 3 );
v_add( X, temp, X, 4 );
/* P[4,4] = P[4,4] - K[4,3] C[3,4] P[4,4] */
m_mult( T1, K, C, 4, 3, 4 );
m_mult( T2, T1, P, 4, 4, 4 );
m_sub( P, T2, P, 4, 4 );
}
/**
* Test matrix transpose.
*/
void test_m_transpose()
{
precision_t data[][4] = {
{ 16, 2, 3, 13 },
{ 5, 11, 10, 8 },
{ 9, 7, 6, 12 },
{ 4, 14, 15, 1 }
};
matrix_t *A = m_initialize(4, 4);
fill_matrix_data(A, data);
matrix_t *B = m_transpose(A);
printf("A = \n");
m_fprint(stdout, A);
printf("B = A' = \n");
m_fprint(stdout, B);
m_free(A);
m_free(B);
}
void Vehicle::vehicle_model(veh_str *vehicl, double Sim_Time)
{
//char dummy[256];
double dp,dr,ds;
//double dp1;
double u[3];
static double u_old,old_phi,old_theta,old_omegac;
double heading_rads;
double head_err, depth_err, pitch_err, prop_err;
static double cosomega, sinomega;
static double costheta, sintheta, tantheta;
static double cosphi, sinphi;
static double rb2t[9]; //(3,3)
static double rt2b[9]; //(3,3)
double *pos;
double *ang;
double *vect;
double dpos[3];
double dang[3];
double dvect[6];
double dvec1[6];
double dvec2[6];
double dvec3[6];
double dvec4[6];
double dxth;
double dxuv[2];
double dxu1[2];
double dxu2[2];
int i;
(*vehicl).phi = (*vehicl).phi*PI/180; // Wei
(*vehicl).theta = (*vehicl).theta*PI/180;
if (init_flag == 1) {
m_invert(Mhinv,Mh,3);
m_mul(Ah,Mhinv,Ch,3,3,3);
m_mul(Bh,Mhinv,Dh,3,3,1);
m_invert(Mdinv,Md,4);
m_mul(Ad,Mdinv,Cd,4,4,4);
m_mul(Bd,Mdinv,Dd,4,4,1);
memset(A, 0, 36*sizeof(double));
A[0] = Xu/(mm - Xud); //(0,0)
A[7] = Ah[0]; //(1,1)=(0,0)
A[11] = Ah[2]; //(1,5)=(0,2)
A[14] = Ad[0]; //(2,2)=(0,0)
A[16] = Ad[1]; //(2,4)=(0,1)
A[21] = Kp/(Ixx - Kpd); //(3,3)
A[23] = (*vehicl).u*mm*zg; //(3,5)
A[26] = Ad[4]; //(4,2)=(1,0)
A[28] = Ad[5]; //(4,4)=(1,1)
A[31] = Ah[6]; //(5,1)=(2,0)
A[35] = Ah[8]; //(5,5)=(2,2)
memset(B, 0, 18*sizeof(double));
B[0] = Xdp/(mm-Xud); //(0,0)
B[1] = Xdr/(mm-Xud); //(0,1)
B[2] = Xds/(mm-Xud); //(0,2)
B[4] = Bh[0]; //(1,1)=(0,0)
B[8] = Bd[0]; //(2,2)=(0,0)
B[9] = Kdp/(Ixx-Kpd); //(3,0)
B[14] = Bd[1]; //(4,2)=(1,0)
B[16] = Bh[2]; //(5,1)=(2,0)
memset(C, 0, 18*sizeof(double));
C[5] = Ah[1]; //(1,2)=(0,1)
C[7] = Ad[3]; //(2,1)=(0,3)
C[9] = Kphi/(Ixx-Kpd); //(3,0)
C[13] = Ad[7]; //(4,1)=(1,3)
C[17] = Ah[7]; //(5,2)=(2,1)
for(i = 0; i < 16; i++) {
Cd1[i]=Cd[i];
}
for(i = 0; i < 9; i++) {
Ch1[i]=Ch[i];
}
d2r = PI/180.0;
rw2a[0] = 1; //(0,0)
rw2a[3] = 0; //(1,0)
rw2a[6] = 0; //(2,0)
u_old = 100;
old_phi = 100;
old_theta = 100;
old_omegac = 100;
}
if(abs((int)((*vehicl).u - u_old))>0.2){
u_old = (*vehicl).u;
Cd1[1] = (*vehicl).u*mm + Zq; //(0,1)
Cd1[4] = (*vehicl).u*(-mm)*xg + Mq; //(1,1)
Cd1[11] = -(*vehicl).u; //(2,3)
Ch1[2] = Yr - (*vehicl).u*mm; //(0,2)
Ch1[5] = Nr - (*vehicl).u*mm*xg; //(1,2)
m_mul(Ah,Mhinv,Ch1,3,3,3);
m_mul(Ad,Mdinv,Cd1,4,4,4);
A[23] = (*vehicl).u*mm*zg; //(3,5)
A[35] = Ah[8]; //(5,5)=(2,2)
A[16] = Ad[1]; //(2,4)=(0,1)
A[28] = Ad[5]; //(4,4)=(1,1)
}
heading_rads = (*vehicl).omega*d2r;
old_phi = (int)((*vehicl).phi);
old_theta = (*vehicl).theta;
old_omegac = (*vehicl).omega;
cosomega = cos(heading_rads);
costheta = cos((*vehicl).theta); //wei
sinomega = sin(heading_rads);
sintheta = sin((*vehicl).theta);
cosphi = cos((*vehicl).phi);
sinphi = sin((*vehicl).phi);
tantheta = tan((*vehicl).theta);
rt2b[0] = cosomega*costheta; //(0,0)
rt2b[1] = sinomega*costheta; //(0,1)
rt2b[2] = -sintheta; //(0,2)
rt2b[3] = -sinomega*cosphi+cosomega*sintheta*sinphi; //(1,0)
rt2b[4] = cosomega*cosphi+sinomega*sintheta*sinphi; //(1,1)
rt2b[5] = costheta*sinphi; //(1,2)
rt2b[6] = sinomega*sinphi+cosomega*sintheta*cosphi; //(2,0)
rt2b[7] = -cosomega*sinphi+sinomega*sintheta*cosphi; //(2,1)
rt2b[8] = costheta*cosphi; //(2,2)
m_transpose(rb2t,rt2b,3,3);
rw2a[1] = sinphi*tantheta; //(0,1)
rw2a[2] = cosphi*tantheta; //(0,2)
rw2a[4] = cosphi; //(1,1)
rw2a[5] = -sinphi; //(1,2)
rw2a[7] = sinphi/costheta; //(2,1)
rw2a[8] = cosphi/costheta; //(2,2)
pos = &((*vehicl).x);
ang = &((*vehicl).phi);
vect = &((*vehicl).u);
//---------------------------heading control---------------------------------
head_err = ((vehicl->omega_c) - (vehicl->omega)); // degrees
head_err = head_err*d2r; // radians
while(head_err>PI)
head_err = head_err - 2.0*PI;
while(head_err<-PI)
head_err = head_err + 2.0*PI;
dr = head_err*-0.2; // Calculate dr
//--------------------------------depth control-----------------------------
depth_err = (*vehicl).z_c - (*vehicl).z;
(*vehicl).theta_c = (-0.772*depth_err);
if((*vehicl).theta_c>0.5)
(*vehicl).theta_c = 0.5;
if((*vehicl).theta_c<-0.5)
(*vehicl).theta_c = -0.5;
pitch_err = ((*vehicl).theta_c - (*vehicl).theta);
ds = ((*vehicl).xth)*Cth + Dth*pitch_err; // Calculate ds
if(ds>0.8) // saturation
ds = 0.8;
if(ds<-0.8)
ds = -0.8;
//-----------------------------propulsion control---------------------------
prop_err = (*vehicl).vel_c - (*vehicl).u;
dp = Cu[0]*(*vehicl).xu1 + Cu[1]*(*vehicl).xu2 + prop_err*Du; // Calculate dp
u[0] = dp;
u[1] = dr;
u[2] = ds;
m_mul(dpos,rb2t,vect,3,3,1);
m_mul(dang,rw2a,(vect+3),3,3,1);
m_mul(dvec1,A,vect,6,6,1);
m_mul(dvec2,B,u,6,3,1);
m_mul(dvec3,C,ang,6,3,1);
m_add(dvec4,dvec1,dvec2,6,1);
m_add(dvect,dvec3,dvec4,6,1);
m_mul(dxu1,Au,&((*vehicl).xu1),2,2,1);
m_muls(dxu2,Bu,prop_err,2,1);
m_add(dxuv,dxu1,dxu2,2,1);
dxth = (Ath*((*vehicl).xth) + Bth*pitch_err); // jay
(*vehicl).xth += dxth*dt;
/*
for (i=0;i<3;i++) { // jay
pos[i] += dpos[i]*dt;
}
*/
pos[0] += dpos[0]*dt; // x
pos[1] += (-1)*dpos[1]*dt; // y, invert because of axis flip
pos[2] += dpos[2]*dt; // z
//
for (i=0;i<2;i++){ //jay
ang[i] += dang[i]*dt; //Wei
}
ang[2] += dang[2]*dt*180.0/PI;
(*vehicl).phi = ang[0]*180/PI; // Wei
(*vehicl).theta = ang[1]*180/PI;
(*vehicl).omega = ang[2];
for (int j=0;j<6;j++){
vect[j] += dvect[j]*dt;
}
(*vehicl).xu1 += dxuv[0]*dt;
(*vehicl).xu2 += dxuv[1]*dt;
//--------------------------------------------------------------------------
if(((*vehicl).src_find_tm) > Sim_Time){ // if not source found
(*vehicl).dsf += ((*vehicl).u)*(dt);} // integrated path length to source
if(((*vehicl).plm_find_tm) > Sim_Time){ // if not plume found
(*vehicl).dpf += ((*vehicl).u)*(dt);} // integrated path length to plume
if(!((*vehicl).src_fnd_flg)){ // if src fnd has not been declared
(*vehicl).dsd += ((*vehicl).u)*(dt);} // integrated path length to plume
while((*vehicl).omega >= 180.0)
(*vehicl).omega = (*vehicl).omega - 360.0;
while((*vehicl).omega < -180.0)
(*vehicl).omega = (*vehicl).omega + 360.0;
}
/**
* ahrs_step() takes the values from the IMU and produces the
* new estimated attitude.
*/
void
ahrs_step(
v_t angles_out,
f_t dt,
f_t p,
f_t q,
f_t r,
f_t ax,
f_t ay,
f_t az,
f_t heading
)
{
m_t C;
m_t A;
m_t AT;
v_t X_dot;
m_t temp;
v_t Xm;
v_t Xe;
/* Construct the quaternion omega matrix */
rotate2omega( A, p, q, r );
/* X_dot = AX */
mv_mult( X_dot, A, X, 4, 4 );
/* X += X_dot dt */
v_scale( X_dot, X_dot, dt, 4 );
v_add( X, X_dot, X, 4 );
v_normq( X, X );
/*
printf( "X =" );
Vprint( X, 4 );
printf( "\n" );
*/
/* AT = transpose(A) */
m_transpose( AT, A, 4, 4 );
/* P = A * P * AT + Q */
m_mult( temp, A, P, 4, 4, 4 );
m_mult( P, temp, AT, 4, 4, 4 );
m_add( P, Q, P, 4, 4 );
compute_c( C, X );
/* Compute the euler angles of the measured attitude */
accel2euler(
Xm,
ax,
ay,
az,
heading
);
/*
* Compute the euler angles of the estimated attitude
* Xe = quat2euler( X )
*/
quat2euler( Xe, X );
/* Kalman filter update */
kalman_update(
P,
R,
C,
X,
Xe,
Xm
);
/* Estimated attitude extraction */
quat2euler( angles_out, X );
}
void transpose_matrix(matrix_t *m)
{
print_matrix("Original Matrix", m);
m_transpose(m);
print_matrix("Transposed Matrix", m);
}
