/* calculate percent error between A and B: 100*norm(A - B)/norm(A) */
double get_percent_error_between_two_mats(gsl_matrix *A, gsl_matrix *B){
int m,n;
double normA, normB, normA_minus_B;
m = A->size1;
n = A->size2;
gsl_matrix *A_minus_B = gsl_matrix_alloc(m,n);
gsl_matrix_memcpy(A_minus_B, A);
gsl_matrix_sub(A_minus_B,B);
normA = matrix_frobenius_norm(A);
normB = matrix_frobenius_norm(B);
normA_minus_B = matrix_frobenius_norm(A_minus_B);
return 100.0*normA_minus_B/normA;
}
Matrix*
Matrix::subtractMatrix(Matrix* b)
{
if(!dimequal(b))
{
cout << "Dimensions do not match" << endl;
return NULL;
}
Matrix* res=new Matrix(row,col);
gsl_matrix_memcpy (res->matrix, matrix);
gsl_matrix_sub(res->matrix,b->matrix);
return res;
}
static int
rm_sub_rm(lua_State *L)
{
mMatReal *a = qlua_checkMatReal(L, 1);
mMatReal *b = qlua_checkMatReal(L, 2);
int al = a->l_size;
int ar = a->r_size;
mMatReal *r = qlua_newMatReal(L, al, ar);
if ((al != b->l_size) || (ar != b->r_size))
return luaL_error(L, "matrix sizes mismatch in m - m");
gsl_matrix_memcpy(r->m, a->m);
gsl_matrix_sub(r->m, b->m);
return 1;
}
float CalcEuclideanNorm(const gsl_matrix * const A,
const gsl_matrix * const B,
gsl_matrix * const R1,
gsl_matrix * const R2,
const gsl_matrix * const Assg,
const gsl_matrix * const AssgT)
{
unsigned int i, j;
float score = 0;
float variance;
// Use AssgT as Assg for multiplication
MulGSLMatrices(Assg, A, R1);
// PrintGSLMatrix(A, "1 - A");
// PrintGSLMatrix(R, "1 - R = Assg x A");
// Compute ScoreMatrix
MulGSLMatrices(R1, AssgT, R2);
// PrintGSLMatrix(AssgT, "AssgT");
// PrintGSLMatrix(R, "2 - R = Assg x A x AssgT");
gsl_matrix_sub(R2, B);
// PrintGSLMatrix(B, "3 - B");
// PrintGSLMatrix(R, "3 - R = Assg x A x AssgT - B");
// Compute variance (for normalization)
variance = CalcVariance(R2);
for(i = 0; i < R2->size1; i++)
{
for(j = 0; j < R2->size2; j++)
{
float val = gsl_matrix_get(R2, i, j);
// fprintf(stderr, "[%d, %d] = %f\n", i, j, val);
//if(variance == (float)0)
score += val*val;
//else
// score += ((val*val)/variance);
//score += val*val;
}
}
//score = sqrt(score);
return score;
}
gsl_matrix* calc_Ws(double s, double tres, double* Qxx_m, double* Qyy_m, double* Qxy_m, double* Qyx_m, int kx, int ky){
gsl_matrix *ws;
ws=gsl_matrix_alloc(kx,kx);
gsl_matrix_set_identity(ws);
gsl_matrix_scale(ws,s);
//Calculate H(s)
gsl_matrix *hs = calc_Hs(s,tres,Qxx_m,Qyy_m,Qxy_m,Qyx_m, kx, ky);
gsl_matrix_sub(ws,hs);
//cleanup
gsl_matrix_free(hs);
return ws;
}
int
solve_PCA(const size_t P, const gsl_matrix * knm,
const gsl_matrix * U, gsl_matrix * alpha,
gsl_matrix * knmt)
{
int status = 0;
const size_t nt = knm->size2; /* number of time stamps */
const size_t nnm = U->size1;
gsl_matrix *R; /* R = knm - U*alpha */
struct timeval tv0, tv1;
double residual; /* || knm - U*alpha || */
int rank;
/* select largest P eigenvectors of SDM */
gsl_matrix_const_view Uv = gsl_matrix_const_submatrix(U, 0, 0, nnm, P);
/* solve: U*alpha = Q */
fprintf(stderr, "solve_PCA: solving PCA problem for alpha...");
gettimeofday(&tv0, NULL);
status = lapack_lls(&Uv.matrix, knm, alpha, &rank);
gettimeofday(&tv1, NULL);
/* compute: knm~ = U*alpha */
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &Uv.matrix, alpha, 0.0, knmt);
/* compute: R = knm - knm~ */
R = gsl_matrix_alloc(nnm, nt);
gsl_matrix_memcpy(R, knm);
gsl_matrix_sub(R, knmt);
residual = norm_fro(R);
fprintf(stderr, "done (%g seconds, status = %d, rank = %d, residual = %.12e)\n",
time_diff(tv0, tv1), status, rank, residual);
gsl_matrix_free(R);
return status;
}
double epidemicGrowthRate(const double theta[numParam], const double r0time, double * eigenvec)
{
gsl_matrix * Fmat = gsl_matrix_calloc(NG*(DS-1)*RG, NG*(DS-1)*RG);
gsl_matrix * Vmat = gsl_matrix_calloc(NG*(DS-1)*RG, NG*(DS-1)*RG);
createNGM(theta, r0time, Fmat, Vmat);
gsl_matrix_sub(Fmat, Vmat);
gsl_eigen_nonsymmv_workspace * w = gsl_eigen_nonsymmv_alloc(NG*(DS-1)*RG);
gsl_vector_complex * eval = gsl_vector_complex_alloc(NG*(DS-1)*RG);
gsl_matrix_complex * evec = gsl_matrix_complex_alloc(NG*(DS-1)*RG, NG*(DS-1)*RG);
gsl_set_error_handler_off();
gsl_eigen_nonsymmv(Fmat, eval, evec, w);
size_t growth_rate_idx = 0;
double growth_rate = -INFINITY;
for(size_t i = 0; i < NG*(DS-1)*RG; i++){
if(GSL_REAL(gsl_vector_complex_get(eval, i)) > growth_rate){
growth_rate_idx = i;
growth_rate = GSL_REAL(gsl_vector_complex_get(eval, i));
}
}
if(eigenvec != NULL){
for(size_t i = 0; i < NG*(DS-1)*RG; i++)
eigenvec[i] = GSL_REAL(gsl_matrix_complex_get(evec, i, growth_rate_idx));
}
gsl_matrix_free(Fmat);
gsl_matrix_free(Vmat);
gsl_vector_complex_free(eval);
gsl_matrix_complex_free(evec);
gsl_eigen_nonsymmv_free(w);
return growth_rate;
}
int minimise(gsl_vector * v_alpha_zero, gsl_vector * v_alpha_A, gsl_matrix * m_V_alpha_zero, gsl_vector * v_d, gsl_matrix * m_D, gsl_vector * v_alpha, gsl_matrix * m_V_alpha, double * chi2) {
size_t height = v_alpha_zero->size;
gsl_vector * v_delta_alpha = gsl_vector_calloc(height);
gsl_vector_memcpy(v_delta_alpha,v_alpha_zero); /* delta_alpha = alpha_zero - alpha_A*/
gsl_vector_sub(v_delta_alpha,v_alpha_A);
gsl_matrix * m_T1 = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,height); /* Temporary matrix */
gsl_matrix * m_T2 = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,TRACK_NBR*CONSTRAINTS); /* Temporary matrix */
gsl_vector * v_T3 = gsl_vector_calloc(TRACK_NBR*CONSTRAINTS); /* Temporary vector */
gsl_matrix * m_V_D = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,TRACK_NBR*CONSTRAINTS);
/* V_D */
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_D,m_V_alpha_zero,0.0,m_T1); /* T1 = D*V_alpha_zero */
gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,m_T1,m_D,0.0,m_T2); /* T2 = T1*D' */
/* Ok */
gsl_blas_dgemv(CblasNoTrans,1.0,m_D,v_delta_alpha,0.0,v_T3); /* T3 = D*delta_alpha */
gsl_vector_add(v_T3,v_d); /* T3 = T3 + d */
/* Ok */
gsl_vector * v_lambda = gsl_vector_calloc(TRACK_NBR*CONSTRAINTS);
/* lambda = inv(T2)*T3 */
int s;
gsl_permutation * p = gsl_permutation_alloc (TRACK_NBR*CONSTRAINTS);
gsl_linalg_LU_decomp (m_T2, p, &s);
gsl_linalg_LU_solve(m_T2,p,v_T3,v_lambda);
/* Ok */
gsl_matrix * m_T4 = gsl_matrix_calloc(height,TRACK_NBR*CONSTRAINTS);
gsl_vector * v_T5 = gsl_vector_calloc(height);
gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,m_V_alpha_zero,m_D,0.0,m_T4); /* T4 = V_alpha_zero*m_D' */
gsl_blas_dgemv(CblasNoTrans,1.0,m_T4,v_lambda,0.0,v_T5); /* T5 = T4*lambda */
gsl_vector_memcpy(v_alpha,v_alpha_A); /* alpha = alpha_A */
gsl_vector_sub(v_alpha,v_T5); /* alpha = alpha_A - T5*/
/* Ok */
/* Compute V_alpha */
gsl_matrix * m_T6 = gsl_matrix_calloc(TRACK_NBR*CONSTRAINTS,TRACK_NBR*CONSTRAINTS);
gsl_matrix * m_T7 = gsl_matrix_calloc(height,TRACK_NBR*CONSTRAINTS);
gsl_matrix * m_T8 = gsl_matrix_calloc(height,height);
gsl_matrix * m_T9 = gsl_matrix_calloc(height,height);
gsl_matrix_memcpy(m_V_alpha,m_V_alpha_zero); /* V_alpha = V_alpha_zero */
/* T6 = inv(T2) */
gsl_linalg_LU_invert(m_T2,p,m_T6); /* See before for LU_decomp(m_T2,...)*/
/* T7 = T4 * T6 */
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_T4,m_T6,0.0,m_T7);
/* T8 = T7 * D */
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_T7,m_D,0.0,m_T8);
/* T9 = T8 * V_alpha_zero */
gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,m_T8,m_V_alpha_zero,0.0,m_T9);
gsl_matrix_sub(m_V_alpha,m_T9);
/* Ok */
/* Compute chi2 */
/* chi2 = lambda' * T3 */
gsl_blas_ddot(v_lambda,v_T3,chi2);
gsl_vector_free(v_T5);
/* Clean the mess */
gsl_matrix_free(m_T1);
gsl_matrix_free(m_T2);
gsl_vector_free(v_T3);
gsl_matrix_free(m_T4);
gsl_matrix_free(m_T6);
gsl_matrix_free(m_T7);
gsl_matrix_free(m_T8);
gsl_matrix_free(m_T9);
gsl_permutation_free(p);
gsl_matrix_free(m_V_D);
gsl_vector_free(v_delta_alpha);
gsl_vector_free(v_lambda);
}
static int MismatchTest(
LatticeTiling *tiling,
gsl_matrix *metric,
const double max_mismatch,
const UINT8 total_ref,
const double mism_hist_ref[MISM_HIST_BINS]
)
{
const size_t n = XLALTotalLatticeTilingDimensions(tiling);
// Create lattice tiling iterator and locator
LatticeTilingIterator *itr = XLALCreateLatticeTilingIterator(tiling, n);
XLAL_CHECK(itr != NULL, XLAL_EFUNC);
LatticeTilingLocator *loc = XLALCreateLatticeTilingLocator(tiling);
XLAL_CHECK(loc != NULL, XLAL_EFUNC);
// Count number of points
const UINT8 total = XLALTotalLatticeTilingPoints(itr);
printf("Number of lattice points: %" LAL_UINT8_FORMAT "\n", total);
XLAL_CHECK(imaxabs(total - total_ref) <= 1, XLAL_EFUNC, "ERROR: |total - total_ref| = |%" LAL_UINT8_FORMAT " - %" LAL_UINT8_FORMAT "| > 1", total, total_ref);
// Get all points
gsl_matrix *GAMAT(points, n, total);
XLAL_CHECK(XLALNextLatticeTilingPoints(itr, &points) == (int)total, XLAL_EFUNC);
XLAL_CHECK(XLALNextLatticeTilingPoint(itr, NULL) == 0, XLAL_EFUNC);
// Initialise mismatch histogram counts
double mism_hist[MISM_HIST_BINS] = {0};
double mism_hist_total = 0, mism_hist_out_of_range = 0;
// Perform 10 injections for every template
{
gsl_matrix *GAMAT(injections, 3, total);
gsl_matrix *GAMAT(nearest, 3, total);
gsl_matrix *GAMAT(temp, 3, total);
RandomParams *rng = XLALCreateRandomParams(total);
XLAL_CHECK(rng != NULL, XLAL_EFUNC);
for (size_t i = 0; i < 10; ++i) {
// Generate random injection points
XLAL_CHECK(XLALRandomLatticeTilingPoints(tiling, 0.0, rng, injections) == XLAL_SUCCESS, XLAL_EFUNC);
// Find nearest lattice template points
XLAL_CHECK(XLALNearestLatticeTilingPoints(loc, injections, &nearest, NULL) == XLAL_SUCCESS, XLAL_EFUNC);
// Compute mismatch between injections
gsl_matrix_sub(nearest, injections);
gsl_blas_dsymm(CblasLeft, CblasUpper, 1.0, metric, nearest, 0.0, temp);
for (size_t j = 0; j < temp->size2; ++j) {
gsl_vector_view temp_j = gsl_matrix_column(temp, j);
gsl_vector_view nearest_j = gsl_matrix_column(nearest, j);
double mismatch = 0.0;
gsl_blas_ddot(&nearest_j.vector, &temp_j.vector, &mismatch);
mismatch /= max_mismatch;
// Increment mismatch histogram counts
++mism_hist_total;
if (mismatch < 0.0 || mismatch > 1.0) {
++mism_hist_out_of_range;
} else {
++mism_hist[lround(floor(mismatch * MISM_HIST_BINS))];
}
}
}
// Cleanup
GFMAT(injections, nearest, temp);
XLALDestroyRandomParams(rng);
}
// Normalise histogram
for (size_t i = 0; i < MISM_HIST_BINS; ++i) {
mism_hist[i] *= MISM_HIST_BINS / mism_hist_total;
}
// Print mismatch histogram and its reference
printf("Mismatch histogram: ");
for (size_t i = 0; i < MISM_HIST_BINS; ++i) {
printf(" %0.3f", mism_hist[i]);
}
printf("\n");
printf("Reference histogram:");
for (size_t i = 0; i < MISM_HIST_BINS; ++i) {
printf(" %0.3f", mism_hist_ref[i]);
}
printf("\n");
// Determine error between mismatch histogram and its reference
double mism_hist_error = 0.0;
for (size_t i = 0; i < MISM_HIST_BINS; ++i) {
mism_hist_error += fabs(mism_hist[i] - mism_hist_ref[i]);
}
mism_hist_error /= MISM_HIST_BINS;
printf("Mismatch histogram error: %0.3e\n", mism_hist_error);
const double mism_hist_error_tol = 5e-2;
if (mism_hist_error >= mism_hist_error_tol) {
XLAL_ERROR(XLAL_EFAILED, "ERROR: mismatch histogram error exceeds %0.3e\n", mism_hist_error_tol);
}
// Check fraction of injections out of histogram range
const double mism_out_of_range = mism_hist_out_of_range / mism_hist_total;
printf("Fraction of points out of histogram range: %0.3e\n", mism_out_of_range);
const double mism_out_of_range_tol = 2e-3;
if (mism_out_of_range > mism_out_of_range_tol) {
XLAL_ERROR(XLAL_EFAILED, "ERROR: fraction of points out of histogram range exceeds %0.3e\n", mism_out_of_range_tol);
}
// Perform 10 injections outside parameter space
{
gsl_matrix *GAMAT(injections, 3, 10);
gsl_matrix *GAMAT(nearest, n, total);
RandomParams *rng = XLALCreateRandomParams(total);
XLAL_CHECK(rng != NULL, XLAL_EFUNC);
// Generate random injection points outside parameter space
XLAL_CHECK(XLALRandomLatticeTilingPoints(tiling, 5.0, rng, injections) == XLAL_SUCCESS, XLAL_EFUNC);
// Find nearest lattice template points
XLAL_CHECK(XLALNearestLatticeTilingPoints(loc, injections, &nearest, NULL) == XLAL_SUCCESS, XLAL_EFUNC);
// Cleanup
GFMAT(injections, nearest);
XLALDestroyRandomParams(rng);
}
// Cleanup
XLALDestroyLatticeTiling(tiling);
XLALDestroyLatticeTilingIterator(itr);
XLALDestroyLatticeTilingLocator(loc);
GFMAT(metric, points);
LALCheckMemoryLeaks();
printf("\n");
fflush(stdout);
return XLAL_SUCCESS;
}
void KF_deriv_steady_C (int *dim, double *sy, double *sZ, double *sT, double *sH,
double *sR, double *sV, double *sQ, double *sa0, double *sP0,
double *tol, int *maxiter,
std::vector<double> *invf, std::vector<double> *vof,
double *dvof, std::vector<double> *dfinvfsq,
gsl_matrix *a_pred, std::vector<gsl_matrix*> *P_pred,
gsl_matrix *K, std::vector<gsl_matrix*> *L,
std::vector<gsl_matrix*> *da_pred,
std::vector< std::vector<gsl_matrix*> > *dP_pred,
std::vector<gsl_matrix*> *dK)
{
//int s, p = dim[1], mp1 = m + 1;
int i, j, k, n = dim[0], m = dim[2],
jm1, r = dim[3], rp1 = r + 1,
conv = 0, counter = 0;
//double v, f, fim1, df[rp1], dv, dtmp; //Kisum, Kim1sum;
double v, f, fim1, dv, dtmp; //Kisum, Kim1sum;
std::vector<double> df(rp1);
//double mll = 0.0; // for debugging
// data and state space model matrices
gsl_vector_view Z = gsl_vector_view_array(sZ, m);
gsl_matrix_view T = gsl_matrix_view_array(sT, m, m);
gsl_matrix_view Q = gsl_matrix_view_array(sQ, m, m);
// storage vectors and matrices
gsl_vector *Vm = gsl_vector_alloc(m);
gsl_vector *Vm_cp = gsl_vector_alloc(m);
gsl_vector *Vm_cp2 = gsl_vector_alloc(m);
gsl_vector *Vm_cp3 = gsl_vector_alloc(m);
gsl_vector *Vm3 = gsl_vector_alloc(m);
gsl_matrix *Mmm = gsl_matrix_alloc(m, m);
gsl_matrix *M1m = gsl_matrix_alloc(1, m);
gsl_matrix *Mm1 = gsl_matrix_alloc(m, 1);
gsl_vector_view a0 = gsl_vector_view_array(sa0, m);
gsl_vector *a_upd = gsl_vector_alloc(m);
gsl_vector_memcpy(a_upd, &a0.vector);
gsl_matrix_view P0 = gsl_matrix_view_array(sP0, m, m);
gsl_matrix *P_upd = gsl_matrix_alloc(m, m);
gsl_matrix_memcpy(P_upd, &P0.matrix);
gsl_vector_view K_irow, m_irow, m2_irow, m3_irow, K_im1row; //Kri;
gsl_matrix_view maux1;
gsl_matrix_view Zm = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), 1, m);
gsl_vector *mZ = gsl_vector_alloc(m);
gsl_vector_memcpy(mZ, &Z.vector);
gsl_vector_scale(mZ, -1.0);
//std::vector<std::vector<gsl_matrix*> *> *da_pred;
std::vector<gsl_matrix*> dP_upd(rp1);
for (j = 0; j < rp1; j++)
{
da_pred[0].at(j) = gsl_matrix_alloc(n, m);
dP_upd.at(j) = gsl_matrix_calloc(m, m);
}
gsl_matrix *da_upd = gsl_matrix_calloc(rp1, m);
// filtering recursions
for (i = 0; i < n; i++)
{
m_irow = gsl_matrix_row(a_pred, i);
gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, a_upd, 0.0, &m_irow.vector);
P_pred[0].at(i) = gsl_matrix_alloc(m, m);
if (conv == 0) {
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, P_upd,
0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, Mmm, &T.matrix,
0.0, P_pred[0].at(i));
gsl_matrix_add(P_pred[0].at(i), &Q.matrix);
} else {
gsl_matrix_memcpy(P_pred[0].at(i), P_pred[0].at(i-1));
}
gsl_blas_ddot(&Z.vector, &m_irow.vector, &v);
v = sy[i] - v;
if (conv == 0) {
gsl_blas_dgemv(CblasNoTrans, 1.0, P_pred[0].at(i), &Z.vector,
0.0, Vm);
gsl_blas_ddot(&Z.vector, Vm, &f);
f += *sH;
invf->at(i) = 1.0 / f;
} else {
invf->at(i) = invf->at(i-1);
}
gsl_vector_memcpy(Vm_cp, Vm);
gsl_vector_memcpy(Vm_cp2, Vm);
gsl_vector_memcpy(Vm_cp3, Vm);
vof->at(i) = v * invf->at(i); // v[i]/f[i];
if (conv == 0) {
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm, 0), m, 1);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &maux1.matrix,
&maux1.matrix, 0.0, Mmm);
gsl_matrix_scale(Mmm, invf->at(i));
gsl_matrix_memcpy(P_upd, P_pred[0].at(i));
gsl_matrix_sub(P_upd, Mmm);
}
gsl_vector_memcpy(a_upd, &m_irow.vector);
gsl_vector_scale(Vm_cp3, vof->at(i));
gsl_vector_add(a_upd, Vm_cp3);
K_irow = gsl_matrix_row(K, i);
gsl_vector_scale(Vm_cp, invf->at(i));
if (conv == 0) {
gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, Vm_cp, 0.0, &K_irow.vector);
} else {
K_im1row = gsl_matrix_row(K, i-1);
gsl_vector_memcpy(&K_irow.vector, &K_im1row.vector);
}
L[0].at(i) = gsl_matrix_alloc(m, m);
if (conv == 0) {
maux1 = gsl_matrix_view_array(gsl_vector_ptr(&K_irow.vector, 0), m, 1);
gsl_matrix_memcpy(L[0].at(i), &T.matrix);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &maux1.matrix,
&Zm.matrix, 1.0, L[0].at(i));
} else {
gsl_matrix_memcpy(L[0].at(i), L[0].at(i-1));
}
// derivatives
dK[0].at(i) = gsl_matrix_alloc(rp1, m);
for (j = 0; j < rp1; j++)
{
k = i + j * n;
m_irow = gsl_matrix_row(da_upd, j);
m2_irow = gsl_matrix_row(da_pred[0].at(j), i);
gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, &m_irow.vector,
0.0, &m2_irow.vector);
gsl_blas_ddot(mZ, &m2_irow.vector, &dv);
(dP_pred[0].at(i)).at(j) = gsl_matrix_alloc(m, m);
if (conv == 0) {
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, dP_upd.at(j),
0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, Mmm, &T.matrix,
0.0, (dP_pred[0].at(i)).at(j));
if (j != 0)
{
jm1 = j - 1;
dtmp = gsl_matrix_get((dP_pred[0].at(i)).at(j), jm1, jm1);
gsl_matrix_set((dP_pred[0].at(i)).at(j), jm1, jm1, dtmp + 1.0);
}
} else {
gsl_matrix_memcpy((dP_pred[0].at(i)).at(j), (dP_pred[0].at(i-1)).at(j));
}
if (conv == 0) {
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &Zm.matrix,
(dP_pred[0].at(i)).at(j), 0.0, M1m);
m_irow = gsl_matrix_row(M1m, 0);
gsl_blas_ddot(&m_irow.vector, &Z.vector, &df[j]);
if (j == 0) {
df[j] += 1.0;
}
}
dvof[k] = (dv * f - v * df[j]) * pow(invf->at(i), 2);
m_irow = gsl_matrix_row(da_upd, j);
gsl_blas_dgemv(CblasNoTrans, vof->at(i), (dP_pred[0].at(i)).at(j), &Z.vector,
0.0, &m_irow.vector);
gsl_vector_add(&m_irow.vector, &m2_irow.vector);
dtmp = -1.0 * df[j] * invf->at(i);
gsl_blas_daxpy(dtmp, Vm_cp3, &m_irow.vector);
gsl_blas_daxpy(dv, Vm_cp, &m_irow.vector);
dfinvfsq->at(k) = df[j] * pow(invf->at(i), 2);
if (conv == 0) {
gsl_matrix_memcpy(dP_upd.at(j), (dP_pred[0].at(i)).at(j));
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, (dP_pred[0].at(i)).at(j),
&Zm.matrix, 0.0, Mm1);
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp, 0), 1, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, Mm1, &maux1.matrix,
1.0, dP_upd.at(j));
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp2, 0), m, 1);
gsl_matrix_memcpy(Mm1, &maux1.matrix);
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp2, 0), 1, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, dfinvfsq->at(k), Mm1,
&maux1.matrix, 1.0, dP_upd.at(j));
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp, 0), m, 1);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix,
&Zm.matrix, 0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, Mmm,
(dP_pred[0].at(i)).at(j), 1.0, dP_upd.at(j));
}
m3_irow = gsl_matrix_row(dK[0].at(i), j);
if (conv == 0) {
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix,
(dP_pred[0].at(i)).at(j), 0.0, Mmm);
gsl_blas_dgemv(CblasNoTrans, 1.0, Mmm, &Z.vector, 0.0, &m3_irow.vector);
gsl_vector_scale(&m3_irow.vector, invf->at(i));
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix,
P_pred[0].at(i), 0.0, Mmm);
gsl_blas_dgemv(CblasNoTrans, 1.0, Mmm, &Z.vector, 0.0, Vm3);
gsl_vector_scale(Vm3, dfinvfsq->at(k));
gsl_vector_sub(&m3_irow.vector, Vm3);
} else {
K_im1row = gsl_matrix_row(dK[0].at(i-1), j);
gsl_vector_memcpy(&m3_irow.vector, &K_im1row.vector);
}
}
// check if convergence to the steady state has been reached
if ((i > 0) & (conv == 0))
{
if (i == 1)
{
fim1 = f + 1.0;
}
if (fabs(f - fim1) < *tol)
{
counter += 1;
}
fim1 = f;
if (counter == *maxiter) {
conv = 1;
dim[5] = i;
}
}
}
// deallocate memory
for (j = 0; j < rp1; j++)
{
gsl_matrix_free(dP_upd.at(j));
}
gsl_vector_free(mZ);
gsl_vector_free(a_upd);
gsl_matrix_free(P_upd);
gsl_vector_free(Vm);
gsl_vector_free(Vm_cp);
gsl_vector_free(Vm_cp2);
gsl_vector_free(Vm_cp3);
gsl_vector_free(Vm3);
gsl_matrix_free(Mmm);
gsl_matrix_free(M1m);
gsl_matrix_free(Mm1);
gsl_matrix_free(da_upd);
}
void KF_deriv_aux_C (int *dim, double *sy, double *sZ, double *sT, double *sH,
double *sR, double *sV, double *sQ, double *sa0, double *sP0,
std::vector<double> *invf, std::vector<double> *vof,
double *dvof, std::vector<double> *dfinvfsq,
gsl_matrix *a_pred, std::vector<gsl_matrix*> *P_pred,
gsl_matrix *K, std::vector<gsl_matrix*> *L,
std::vector<gsl_matrix*> *da_pred,
std::vector< std::vector<gsl_matrix*> > *dP_pred,
std::vector<gsl_matrix*> *dK)
{
//int s, p = dim[1], mp1 = m + 1;
int i, j, k, n = dim[0], m = dim[2],
jm1, r = dim[3], rp1 = r + 1;
double v, f, df, dv, dtmp;
// data and state space model matrices
gsl_vector_view Z = gsl_vector_view_array(sZ, m);
gsl_matrix_view T = gsl_matrix_view_array(sT, m, m);
gsl_matrix_view Q = gsl_matrix_view_array(sQ, m, m);
// storage vectors and matrices
gsl_vector *Vm = gsl_vector_alloc(m);
gsl_vector *Vm_cp = gsl_vector_alloc(m);
gsl_vector *Vm_cp2 = gsl_vector_alloc(m);
gsl_vector *Vm3 = gsl_vector_alloc(m);
gsl_matrix *Mmm = gsl_matrix_alloc(m, m);
gsl_matrix *M1m = gsl_matrix_alloc(1, m);
gsl_matrix *Mm1 = gsl_matrix_alloc(m, 1);
gsl_vector_view a0 = gsl_vector_view_array(sa0, m);
gsl_vector *a_upd = gsl_vector_alloc(m);
gsl_vector_memcpy(a_upd, &a0.vector);
gsl_matrix_view P0 = gsl_matrix_view_array(sP0, m, m);
gsl_matrix *P_upd = gsl_matrix_alloc(m, m);
gsl_matrix_memcpy(P_upd, &P0.matrix);
gsl_vector_view K_irow, m_irow, m2_irow, m3_irow;
gsl_matrix_view maux1;
gsl_matrix_view Zm = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), 1, m);
gsl_vector *mZ = gsl_vector_alloc(m);
gsl_vector_memcpy(mZ, &Z.vector);
gsl_vector_scale(mZ, -1.0);
std::vector<gsl_matrix*> dP_upd(rp1);
for (j = 0; j < rp1; j++)
{
da_pred[0].at(j) = gsl_matrix_alloc(n, m);
dP_upd.at(j) = gsl_matrix_calloc(m, m);
}
gsl_matrix *da_upd = gsl_matrix_calloc(rp1, m);
// filtering recursions
for (i = 0; i < n; i++)
{
m_irow = gsl_matrix_row(a_pred, i);
gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, a_upd, 0.0, &m_irow.vector);
P_pred[0].at(i) = gsl_matrix_alloc(m, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, P_upd,
0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, Mmm, &T.matrix,
0.0, P_pred[0].at(i));
gsl_matrix_add(P_pred[0].at(i), &Q.matrix);
gsl_blas_ddot(&Z.vector, &m_irow.vector, &v);
v = sy[i] - v;
gsl_blas_dgemv(CblasNoTrans, 1.0, P_pred[0].at(i), &Z.vector,
0.0, Vm);
gsl_blas_ddot(&Z.vector, Vm, &f);
f += *sH;
gsl_vector_memcpy(Vm_cp, Vm);
gsl_vector_memcpy(Vm_cp2, Vm);
invf->at(i) = 1.0 / f;
vof->at(i) = v * invf->at(i); // v[i]/f[i];
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm, 0), m, 1);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &maux1.matrix,
&maux1.matrix, 0.0, Mmm);
gsl_matrix_scale(Mmm, invf->at(i));
gsl_vector_memcpy(a_upd, &m_irow.vector);
gsl_vector_scale(Vm, vof->at(i));
gsl_vector_add(a_upd, Vm);
gsl_matrix_memcpy(P_upd, P_pred[0].at(i));
gsl_matrix_sub(P_upd, Mmm);
K_irow = gsl_matrix_row(K, i);
gsl_vector_scale(Vm_cp, invf->at(i));
gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, Vm_cp, 0.0, &K_irow.vector);
L[0].at(i) = gsl_matrix_alloc(m, m);
maux1 = gsl_matrix_view_array(gsl_vector_ptr(&K_irow.vector, 0), m, 1);
gsl_matrix_memcpy(L[0].at(i), &T.matrix);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &maux1.matrix,
&Zm.matrix, 1.0, L[0].at(i));
// derivatives
dK[0].at(i) = gsl_matrix_alloc(rp1, m);
for (j = 0; j < rp1; j++)
{
k = i + j * n;
m_irow = gsl_matrix_row(da_upd, j);
m2_irow = gsl_matrix_row(da_pred[0].at(j), i);
gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, &m_irow.vector,
0.0, &m2_irow.vector);
gsl_blas_ddot(mZ, &m2_irow.vector, &dv);
(dP_pred[0].at(i)).at(j) = gsl_matrix_alloc(m, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, dP_upd.at(j),
0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, Mmm, &T.matrix,
0.0, (dP_pred[0].at(i)).at(j));
if (j != 0)
{
jm1 = j - 1;
dtmp = gsl_matrix_get((dP_pred[0].at(i)).at(j), jm1, jm1);
gsl_matrix_set((dP_pred[0].at(i)).at(j), jm1, jm1, dtmp + 1.0);
}
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &Zm.matrix,
(dP_pred[0].at(i)).at(j), 0.0, M1m);
m_irow = gsl_matrix_row(M1m, 0);
gsl_blas_ddot(&m_irow.vector, &Z.vector, &df);
if (j == 0) {
df += 1.0;
}
dvof[k] = (dv * f - v * df) * pow(invf->at(i), 2);
m_irow = gsl_matrix_row(da_upd, j);
gsl_blas_dgemv(CblasNoTrans, vof->at(i), (dP_pred[0].at(i)).at(j), &Z.vector,
0.0, &m_irow.vector);
gsl_vector_add(&m_irow.vector, &m2_irow.vector);
dtmp = -1.0 * df * invf->at(i);
gsl_blas_daxpy(dtmp, Vm, &m_irow.vector);
gsl_blas_daxpy(dv, Vm_cp, &m_irow.vector);
gsl_matrix_memcpy(dP_upd.at(j), (dP_pred[0].at(i)).at(j));
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, (dP_pred[0].at(i)).at(j),
&Zm.matrix, 0.0, Mm1);
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp, 0), 1, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, Mm1, &maux1.matrix,
1.0, dP_upd.at(j));
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp2, 0), m, 1);
gsl_matrix_memcpy(Mm1, &maux1.matrix);
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp2, 0), 1, m);
dfinvfsq->at(k) = df * pow(invf->at(i), 2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, dfinvfsq->at(k), Mm1,
&maux1.matrix, 1.0, dP_upd.at(j));
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp, 0), m, 1);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix,
&Zm.matrix, 0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, Mmm,
(dP_pred[0].at(i)).at(j), 1.0, dP_upd.at(j));
m3_irow = gsl_matrix_row(dK[0].at(i), j);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix,
(dP_pred[0].at(i)).at(j), 0.0, Mmm);
gsl_blas_dgemv(CblasNoTrans, 1.0, Mmm, &Z.vector, 0.0, &m3_irow.vector);
gsl_vector_scale(&m3_irow.vector, invf->at(i));
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix,
P_pred[0].at(i), 0.0, Mmm);
gsl_blas_dgemv(CblasNoTrans, 1.0, Mmm, &Z.vector, 0.0, Vm3);
gsl_vector_scale(Vm3, dfinvfsq->at(k));
gsl_vector_sub(&m3_irow.vector, Vm3);
}
}
// deallocate memory
for (j = 0; j < rp1; j++)
{
gsl_matrix_free(dP_upd.at(j));
}
gsl_vector_free(mZ);
gsl_vector_free(a_upd);
gsl_matrix_free(P_upd);
gsl_vector_free(Vm);
gsl_vector_free(Vm_cp);
gsl_vector_free(Vm_cp2);
gsl_vector_free(Vm3);
gsl_matrix_free(Mmm);
gsl_matrix_free(M1m);
gsl_matrix_free(Mm1);
gsl_matrix_free(da_upd);
}
void infomax(gsl_matrix *x_white, gsl_matrix *weights, gsl_matrix *S, int verbose){
/*Computes ICA infomax in whitened data
Decomposes x_white as x_white=AS
*Input
x_white: whitened data (Use PCAwhiten)
*Output
A : mixing matrix
S : source matrix
*/
// int verbose = 1; //true
size_t NCOMP = x_white->size1;
size_t NVOX = x_white->size2;
//getting permutation vector
const gsl_rng_type * T;
gsl_rng * r;
gsl_permutation * p = gsl_permutation_alloc (NVOX);
// gsl_rng_env_setup();
T = gsl_rng_default;
r = gsl_rng_alloc (T);
gsl_permutation_init (p);
gsl_matrix *old_weights = gsl_matrix_alloc(NCOMP,NCOMP);
gsl_matrix *bias = gsl_matrix_calloc(NCOMP, 1);
gsl_matrix *d_weights = gsl_matrix_calloc(NCOMP,NCOMP);
gsl_matrix *temp_change = gsl_matrix_alloc(NCOMP,NCOMP);
gsl_matrix *old_d_weights = gsl_matrix_calloc(NCOMP,NCOMP);
gsl_matrix *shuffled_x_white = gsl_matrix_calloc(NCOMP,x_white->size2);
gsl_matrix_memcpy(shuffled_x_white, x_white);
gsl_matrix_set_identity(weights);
gsl_matrix_set_identity(old_weights);
double lrate = 0.005/log((double)NCOMP);
double change=1;
double angle_delta =0;
size_t step = 1;
int error = 0;
while( (step < MAX_STEP) && (change > W_STOP)){
error = w_update(weights, x_white, bias,
shuffled_x_white, p, r, lrate);
if (error==1 || error==2){
// It blowed up! RESTART!
step = 1;
// change = 1;
error = 0;
lrate *= ANNEAL;
gsl_matrix_set_identity(weights);
gsl_matrix_set_identity(old_weights);
gsl_matrix_set_zero(d_weights);
gsl_matrix_set_zero(old_d_weights);
gsl_matrix_set_zero(bias);
if (lrate > MIN_LRATE){
printf("\nLowering learning rate to %g and starting again.\n",lrate);
}
else{
printf("\nMatrix may not be invertible");
}
}
else if (error==0){
gsl_matrix_memcpy(d_weights, weights);
gsl_matrix_sub(d_weights, old_weights);
change = matrix_norm(d_weights);
if (step > 2){
// Compute angle delta
gsl_matrix_memcpy(temp_change, d_weights);
gsl_matrix_mul_elements(temp_change, old_d_weights);
angle_delta = acos(matrix_sum(temp_change) / sqrt(matrix_norm(d_weights)*(matrix_norm(old_d_weights))));
angle_delta *= (180.0 / M_PI);
}
gsl_matrix_memcpy(old_weights, weights);
if (angle_delta > 60){
lrate *= ANNEAL;
gsl_matrix_memcpy(old_d_weights, d_weights);
} else if (step==1) {
gsl_matrix_memcpy(old_d_weights, d_weights);
}
if ((verbose && (step % 10)== 0) || change < W_STOP){
printf("\nStep %zu: Lrate %.1e, Wchange %.1e, Angle %.2f",
step, lrate, change, angle_delta);
}
step ++;
}
}
matrix_mmul(weights, x_white, S);
gsl_matrix_free(old_d_weights);
gsl_matrix_free(old_weights);
gsl_matrix_free(bias);
gsl_matrix_free(d_weights);
gsl_matrix_free(shuffled_x_white);
gsl_rng_free (r);
gsl_permutation_free (p);
}
matrix operator-(const matrix& m1, const matrix& m2) {
matrix out(m1);
gsl_matrix_sub(out.ptr_, m2.ptr_);
return out;
}
/* test if A = Q S Z^t */
void
test_schur(gsl_matrix *A, gsl_matrix *S, gsl_matrix *Q, gsl_matrix *Z)
{
const size_t N = A->size1;
gsl_matrix *T1, *T2;
size_t i, j, k;
double lhs, rhs;
double abserr;
T1 = gsl_matrix_alloc(N, N);
T2 = gsl_matrix_alloc(N, N);
/* compute T1 = S Z^t */
gsl_blas_dgemm(CblasNoTrans,
CblasTrans,
1.0,
S,
Z,
0.0,
T1);
/* compute T2 = Q T1 = Q S Z^t */
gsl_blas_dgemm(CblasNoTrans,
CblasNoTrans,
1.0,
Q,
T1,
0.0,
T2);
k = 0;
for (i = 0; i < N; ++i)
{
for (j = 0; j < N; ++j)
{
lhs = gsl_matrix_get(A, i, j);
rhs = gsl_matrix_get(T2, i, j);
abserr = fabs(lhs - rhs);
if (abserr > 1.0e-6)
++k;
}
}
if (k)
{
printf("==== CASE %lu ===========================\n\n", count);
print_matrix(A, "A");
printf("=== Schur Form matrix ===\n");
print_matrix(S, "S");
printf("=== Left Schur matrix ===\n");
print_matrix(Q, "Q");
printf("=== Right Schur matrix ===\n");
print_matrix(Z, "Z");
printf("=== Q S Z^t ===\n");
print_matrix(T1, "Q S Z^t");
printf("=== A - Q S Z^t ===\n");
gsl_matrix_sub(T2, A);
print_matrix(T1, "A - Q S Z^t");
printf("=========================================\n\n");
}
gsl_matrix_free(T1);
gsl_matrix_free(T2);
} /* test_schur() */
/// subtract a matrix from this
/// @param M :: A matrix
GSLMatrix &GSLMatrix::operator-=(const GSLMatrix &M) {
gsl_matrix_sub(&m_view.matrix, M.gsl());
return *this;
}
/**
this method subtracts the matrix that is passed in as a parameter from the current matrix. In addition, it scales, both matrices by the two
numbers that are passed in as parameters:
*this = a * *this - b * other.
*/
void Matrix::sub(const Matrix& other, double scaleA, double scaleB){
if (scaleA == 1.0 && scaleB == 1)
gsl_matrix_sub(this->matrix, other.matrix);
else
gsl_matrix_add_scaled(this->matrix, other.matrix, scaleA, -scaleB);
}
int model::predict(const dataset &tds, gsl_matrix **pp)
{
int ret = -1;
gsl_matrix *mat = NULL;
gsl_matrix *ptv = NULL;
gsl_matrix *km1 = NULL;
gsl_matrix *km2 = NULL;
gsl_matrix *res = NULL;
gsl_matrix *stm = NULL;
gsl_vector_view avg_col;
gsl_vector_view dv;
if (tds.ins_num() <= 0 || tds.fea_num() != (int)_col_mean->size) {
ULIB_FATAL("invalid test dimensions, (ins_num=%d,fea_num=%d)",
tds.ins_num(), tds.fea_num());
goto done;
}
mat = gsl_matrix_alloc(tds.ins_num(), tds.fea_num());
if (mat == NULL) {
ULIB_FATAL("couldn't allocate test feature matrix");
goto done;
}
ptv = gsl_matrix_alloc(tds.ins_num(), 2);
if (ptv == NULL) {
ULIB_FATAL("couldn't allocate prediction matrix");
goto done;
}
if (tds.get_matrix(mat)) {
ULIB_FATAL("couldn't get test matrix");
goto done;
}
dbg_print_mat(mat, "Test Matrix:");
zero_out_mat(mat);
norm_mat(mat);
dbg_print_mat(mat, "Normalized Test Matrix:");
km1 = comp_kern_mat(mat, _fm, _kern);
if (km1 == NULL) {
ULIB_FATAL("couldn't compute test1 kernel matrix");
goto done;
}
dbg_print_mat(km1, "Test Kernel Matrix:");
km2 = comp_kern_mat(mat, mat, _kern);
if (km2 == NULL) {
ULIB_FATAL("couldn't compute test2 kernel matrix");
goto done;
}
dbg_print_mat(km1, "Test Kernel Matrix:");
dv = gsl_matrix_diagonal(km2);
res = gsl_matrix_alloc(km1->size1, _ikm->size2);
if (res == NULL) {
ULIB_FATAL("couldn't allocate temporary matrix");
goto done;
}
stm = gsl_matrix_alloc(km2->size1, km2->size2);
if (stm == NULL) {
ULIB_FATAL("couldn't allocate std matrix");
goto done;
}
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, km1, _ikm, 0.0, res);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, res, km1, 0.0, stm);
gsl_matrix_sub(km2, stm);
dbg_print_mat(res, "Predictive Matrix:");
avg_col = gsl_matrix_column(ptv, 0);
gsl_blas_dgemv(CblasNoTrans, 1.0, res, _tv, 0.0, &avg_col.vector);
gsl_vector_add_constant(&avg_col.vector, _t_avg);
gsl_matrix_scale(km2, _t_std*_t_std);
gsl_vector_add_constant(&dv.vector, _noise_var);
for (size_t i = 0; i < km2->size1; ++i)
gsl_matrix_set(ptv, i, 1, sqrt(gsl_vector_get(&dv.vector, i)));
*pp = ptv;
ptv = NULL;
ret = 0;
done:
gsl_matrix_free(mat);
gsl_matrix_free(ptv);
gsl_matrix_free(km1);
gsl_matrix_free(km2);
gsl_matrix_free(res);
gsl_matrix_free(stm);
return ret;
}
int main() {
srand(time(NULL));
int states = 6;
int unfiltered_states = 3;
gsl_matrix *state_mean = gsl_matrix_calloc(states,1);
gsl_matrix_set(state_mean, 0,0, 3);
gsl_matrix_set(state_mean, 1,0, 3);
gsl_matrix *state_covariance = gsl_matrix_calloc(states,states);
gsl_matrix *observation_mean = gsl_matrix_calloc(states,1);
gsl_matrix *observation_covariance = gsl_matrix_calloc(states,states);
//gsl_matrix_set(observation_covariance, 0, 0, hdop);
//gsl_matrix_set(observation_covariance, 1, 1, hdop);
//gsl_matrix_set(observation_covariance, 2, 2, vert_err);
//gsl_matrix_set(observation_covariance, 3, 3, (2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 4, 4, (2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 5, 5, (2*vert_err)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 0, 3, timestep*(2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 3, 0, timestep*(2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 1, 4, timestep*(2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 4, 1, timestep*(2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 2, 5, timestep*(2*vert_err)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 5, 2, timestep*(2*vert_err)/pow(timestep,2));
gsl_matrix *observation_transformation = gsl_matrix_calloc(states,states);
gsl_matrix *estimate_mean = gsl_matrix_calloc(states,1);
gsl_matrix *estimate_covariance = gsl_matrix_calloc(states,states);
gsl_matrix *kalman_gain = gsl_matrix_calloc(states,states);
gsl_matrix *temp21a = gsl_matrix_calloc(states,1);
gsl_matrix *temp21b = gsl_matrix_calloc(states,1);
gsl_matrix *temp22a = gsl_matrix_calloc(states,states);
gsl_matrix *temp22b = gsl_matrix_calloc(states,states);
gsl_matrix *predict = gsl_matrix_calloc(states,states);
//gsl_matrix_set(predict, 0, 0, 1);
//gsl_matrix_set(predict, 1, 1, 1);
//gsl_matrix_set(predict, 2, 2, 1);
gsl_matrix_set(predict, 3, 3, 1);
gsl_matrix_set(predict, 4, 4, 1);
gsl_matrix_set(predict, 5, 5, 1);
//gsl_matrix_set(predict, 0, 3, timestep);
//gsl_matrix_set(predict, 1, 4, timestep);
//gsl_matrix_set(predict, 2, 5, timestep);
gsl_matrix *control = gsl_matrix_calloc(states, unfiltered_states);
//gsl_matrix_set(control, 0, 0, 0.5*pow(timestep,2));
//gsl_matrix_set(control, 1, 1, 0.5*pow(timestep,2));
//gsl_matrix_set(control, 2, 2, 0.5*pow(timestep,2));
//gsl_matrix_set(control, 3, 0, timestep);
//gsl_matrix_set(control, 4, 1, timestep);
//gsl_matrix_set(control, 5, 2, timestep);
gsl_vector *acceleration = gsl_vector_calloc(unfiltered_states);
gsl_vector *velocity = gsl_vector_calloc(unfiltered_states);
gsl_vector *location = gsl_vector_calloc(unfiltered_states);
gsl_vector *delta_location = gsl_vector_calloc(unfiltered_states);
gsl_vector *temp_location = gsl_vector_calloc(unfiltered_states);
double timestep;
int hdop;
int vert_err;
int s;
double error;
for (int t = 1; t < 1000000; t++) {
timestep = 1;
gsl_vector_set(acceleration, 0, (rand()%101)-52);
gsl_vector_set(acceleration, 1, (rand()%101)-46);
gsl_vector_set(acceleration, 2, (rand()%101)-54);
gsl_vector_add(velocity, acceleration);
gsl_vector_add(location, velocity);
gsl_vector_memcpy(delta_location, location);
gsl_vector_sub(delta_location, temp_location);
gsl_vector_memcpy(temp_location, location);
gsl_matrix_set(predict, 0, 3, timestep);
gsl_matrix_set(predict, 1, 4, timestep);
gsl_matrix_set(predict, 2, 5, timestep);
gsl_matrix_set(control, 0, 0, 0.5*pow(timestep,2));
gsl_matrix_set(control, 1, 1, 0.5*pow(timestep,2));
gsl_matrix_set(control, 2, 2, 0.5*pow(timestep,2));
gsl_matrix_set(control, 3, 0, timestep);
gsl_matrix_set(control, 4, 1, timestep);
gsl_matrix_set(control, 5, 2, timestep);
hdop = 5;
vert_err = 5;
gsl_matrix_set(observation_covariance, 0, 0, hdop);
gsl_matrix_set(observation_covariance, 1, 1, hdop);
gsl_matrix_set(observation_covariance, 2, 2, vert_err);
gsl_matrix_set(observation_covariance, 3, 3, (2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 4, 4, (2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 5, 5, (2*vert_err)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 0, 3, timestep*(2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 3, 0, timestep*(2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 1, 4, timestep*(2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 4, 1, timestep*(2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 2, 5, timestep*(2*vert_err)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 5, 2, timestep*(2*vert_err)/pow(timestep,2));
//observation_transformation = observation_mean[k] * pseudoinverse(state_mean[k-1])
gsl_matrix_set_zero(temp21a);
gsl_matrix_set(temp21a, 0, 0, gsl_vector_get(delta_location,0));
gsl_matrix_set(temp21a, 1, 0, gsl_vector_get(delta_location,1));
gsl_matrix_set(temp21a, 2, 0, gsl_vector_get(delta_location,2));
gsl_matrix_set(temp21a, 3, 0, gsl_vector_get(velocity,0));
gsl_matrix_set(temp21a, 4, 0, gsl_vector_get(velocity,1));
gsl_matrix_set(temp21a, 5, 0, gsl_vector_get(velocity,2));
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, temp21a, pseudo_inverse(state_mean), 0, observation_transformation);
//observation_mean[k] = observation_transformation * state_mean[k-1]
gsl_matrix_memcpy(temp21a, state_mean);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, observation_transformation, temp21a, 0, observation_mean);
//observation_covariance[k] = observation_transformation * state_covariance * transpose(observation_transformation)
/* gsl_matrix_set_zero(temp22a);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, state_covariance, observation_transformation, 0, temp22a); //notice observation_transformation is transposed
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, observation_transformation, temp22a, 0, observation_covariance);
*/
gsl_matrix_set(observation_covariance, 0, 3, timestep*3);
gsl_matrix_set(observation_covariance, 3, 0, timestep*3);
gsl_matrix_set(observation_covariance, 1, 4, timestep*5);
gsl_matrix_set(observation_covariance, 4, 1, timestep*5);
gsl_matrix_set(observation_covariance, 2, 5, timestep*6);
gsl_matrix_set(observation_covariance, 5, 2, timestep*6);
//estimate_mean = predict * state_mean + control * acceleration;
gsl_matrix_set_zero(estimate_mean);
gsl_matrix_set_zero(temp21a);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, predict, state_mean, 0, estimate_mean);
gsl_matrix_view test = gsl_matrix_view_vector(acceleration, unfiltered_states, 1);
gsl_matrix * tmp = &test.matrix;
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, control, tmp, 0, temp21a);
gsl_matrix_add(estimate_mean, temp21a);
//estimate_covariance = predict * state_covariance * transpose(predict) + noise;
gsl_matrix_set_zero(estimate_covariance);
gsl_matrix_set_zero(temp22a);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, state_covariance, predict, 0, temp22a); //notice predict is transposed
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, predict, temp22a, 0, estimate_covariance);
gsl_matrix_add_constant(estimate_covariance, 0.1); //adxl345 noise, this is completely wrong
//kalman_gain = estimate_covariance * pseudoinverse(estimate_covariance + observation_covariance);
gsl_matrix_set_zero(kalman_gain);
gsl_matrix_set_zero(temp22a);
gsl_matrix_memcpy(temp22a, observation_covariance);
gsl_matrix_add(temp22a, estimate_covariance);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, estimate_covariance, pseudo_inverse(temp22a), 0, kalman_gain);
//state_mean = estimate_mean + kalman_gain * ( observation_mean - estimate_mean );
gsl_matrix_set_zero(state_mean);
gsl_matrix_set_zero(temp21a);
gsl_matrix_memcpy(temp21a, observation_mean);
gsl_matrix_sub(temp21a, estimate_mean);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, kalman_gain, temp21a, 0, state_mean);
gsl_matrix_add(state_mean, estimate_mean);
//state_covariance = estimate_covariance - kalman_gain * ( estimate_covariance );
gsl_matrix_set_zero(state_covariance);
gsl_matrix_set_zero(temp22a);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, kalman_gain, estimate_covariance, 0, temp22a);
gsl_matrix_add(state_covariance, estimate_covariance);
gsl_matrix_sub(state_covariance, temp22a);
printf("state_mean:");
gsl_matrix_fprintf(stdout, state_mean, "%f");
//printf("state_covariance:");
//gsl_matrix_fprintf(stdout, state_covariance, "%f");
printf("observation_mean:");
gsl_matrix_fprintf(stdout, observation_mean, "%f");
//printf("observation_covariance:");
//gsl_matrix_fprintf(stdout, observation_covariance, "%f");
printf("estimate_mean:");
gsl_matrix_fprintf(stdout, estimate_mean, "%f");
//printf("estimate_covariance:");
//gsl_matrix_fprintf(stdout, estimate_covariance, "%f");
gsl_matrix_set_zero(temp21a);
gsl_matrix_memcpy(temp21a, observation_mean);
gsl_matrix_sub(temp21a, state_mean);
gsl_matrix_div_elements(temp21a, observation_mean);
gsl_matrix_mul_elements(temp21a, temp21a);
for (int i = states-1; i >= 0; i--) {
error += gsl_matrix_get(temp21a, i, 0);
}
printf("error: %f\n", error);
printf("error/time: %f\n", error/t);
printf("\n");
usleep(1000000);
}
}
/* Performs LSE of consequent parameters for all branches in the network
*
* PARAMETERS: A ---> predictor variables matrix
* y ---> expected results for this sample set (P in total)
*
* PRE: A != NULL
* y != NULL
*
* POS: result != NULL && best fit consequent parameters vector returned
* or
* result == NULL
*/
static gsl_vector *
anfis_fit_linear (const gsl_matrix *A, const gsl_vector *y, size_t P, size_t M)
{
gsl_matrix *S = NULL, /* Covariance matrix */
*Snew = NULL,
*Saux = NULL;
gsl_vector *X = NULL, /* Future best fit parameters */
*Xnew = NULL;
unsigned int i = 0;
double den = 0.0, factor = 0.0;
assert (A != NULL);
assert (y != NULL);
/* Generating necessary workspace */
S = gsl_matrix_alloc (M,M);
if (S == NULL)
goto exit_point;
Snew = gsl_matrix_calloc (M,M);
if (Snew == NULL)
goto exit_point;
Saux = gsl_matrix_calloc (M,M);
if (Saux == NULL)
goto exit_point;
Xnew = gsl_vector_alloc (M);
if (Xnew == NULL)
goto exit_point;
X = gsl_vector_calloc (M);
if (X == NULL)
goto exit_point;
/* S = γ*Id , where γ is a large number */
gsl_matrix_set_identity (S);
gsl_matrix_scale (S, _gamma);
/* Performing Least Square Estimation */
for (i=0 ; i < P ; i++) {
/* Matrix A i-th row (row At_i+1 in Jang's paper) */
gsl_vector_const_view A_i = gsl_matrix_const_row (A, i);
/* Snew = S(i) * A_i+1 * At_i+1 * S(i) */
calc_num (S, &(A_i.vector), Snew, Saux, Xnew, M);
/* scale = At_i+1 * S(i) * A_i+1 */
den = calc_den (S, &(A_i.vector), Xnew);
/* Snew = Snew / (1+scale) */
gsl_matrix_scale (Snew, 1.0 / (1.0+den));
/* S(i+1) = S(i) - Snew */
gsl_matrix_sub (S, Snew);
/* factor = At_i+1 * X(i) */
gsl_blas_ddot (&(A_i.vector), X, &factor);
/* factor = yt_i+1 - factor */
factor = gsl_vector_get (y, i) - factor;
/* Xnew = S(i+1) * A_i+1 */
gsl_blas_dgemv (CblasNoTrans, 1.0, S, &(A_i.vector), 0.0, Xnew);
/* Xnew = Xnew * factor */
gsl_vector_scale (Xnew, factor);
/* X(i+1) = X(i) + Xnew */
gsl_vector_add (X, Xnew);
}
exit_point:
if (S != NULL) { gsl_matrix_free (S); S = NULL;}
if (Snew != NULL) { gsl_matrix_free (Snew); Snew = NULL;}
if (Saux != NULL) { gsl_matrix_free (Saux); Saux = NULL;}
if (Xnew != NULL) { gsl_vector_free (Xnew); Xnew = NULL;}
return X;
}
/** @a Main Kalman Algorithm - Final process Update position and update P(k+1|k+1)
* /* General correction algorithm equations, as per main report document
*
*
Correction equations:
1) Pose correction
x(k + 1|k + 1) = x(k + 1|k) + w(k + 1).v(k + 1) (5.10)
where:
x(k + 1|k), predicted pose from Stage 1 of the EKF
W(k + 1), Kalman Gain
v(k + 1), innovation vector obtained from
feature matching stage
definition of the terms of the equation 5.10:
w(k + 1) = R(k + 1|k).GgT(k + 1).S^−1(k + 1) (5.11)
v(k + 1) = [ v1(k+1) ] (5.6)
[ v2(k+1) ]
...
[ vn(k+1) ]
where:
R(k + 1|k), pose covariance matrix
Gg(k + 1), aggregated pose gradient matrix obtained from
feature prediction stage
S(k + 1), aggregated feature covariance
definition of the terms of the equation 5.11:
Gg(k + 1) = [ Gg1(k+1) ] (5.7)
[ Gg2(k+1) ]
...
[ Ggn(k+1) ]
S(k + 1) = Gg(k+1).R(k + 1|k).GgT(k+1) + Q(k + 1) (5.8)
where:
Q(k + 1), aggregated feature covariace matrix
Q(k + 1) = [ D1 0 ... 0 ]
[ 0 D2 ... 0 ]
[ ... ... ... ...]
[ 0 0 ... Dn ]
Dn, diagonal elements of the features covariace
2) Covariance correction
R(k + 1|k + 1) = R(k + 1|k) − w(k + 1).S(k + 1).wT(k + 1) (5.12)
**/
void Nav_MotionEstimator::Nav_EKFStage3Correction( )
{
// features matched during
boost::ptr_list<MatchedFeatures_C>::iterator MatchedIterator;
int Featincorrection = 0;
cout << "FEATURE MATCHED: " << FeatMatched.size() << endl;
/** Count the number of mature features that can be used in the correction */
for (MatchedIterator = FeatMatched.begin(); MatchedIterator != FeatMatched.end(); MatchedIterator++)
{
if ( (*MatchedIterator).InMapFeat->isMature () ) {
Featincorrection++;
}
}
cout << " CORRECTION WITH : " << Featincorrection << endl;
if ( Featincorrection == 0 ) return;
/* Allocate matrixes */
gsl_matrix * Vk = gsl_matrix_alloc ( 2*Featincorrection, 1);
gsl_matrix * Gg = gsl_matrix_alloc ( 2 * Featincorrection, 3);
gsl_matrix * GgT = gsl_matrix_alloc ( 3, 2 * Featincorrection );
gsl_matrix * W = gsl_matrix_alloc ( 3, 2 * Featincorrection );
gsl_matrix * WT = gsl_matrix_alloc ( 2 * Featincorrection, 3 );
gsl_matrix * QK = gsl_matrix_alloc(Featincorrection * 2, Featincorrection * 2);
gsl_matrix * SK = gsl_matrix_alloc(Featincorrection * 2, Featincorrection * 2);
gsl_matrix * SKinv = gsl_matrix_alloc(Featincorrection * 2, Featincorrection * 2);
gsl_matrix * mat2nx3 = gsl_matrix_alloc(Featincorrection * 2, 3);
gsl_matrix * mat3xn2 = gsl_matrix_alloc(3, Featincorrection * 2);
gsl_matrix * mat3x3 = gsl_matrix_alloc(3, 3);
gsl_matrix * mat3x1 = gsl_matrix_alloc(3, 1);
// fill the matrixes with the relevant data
int InnovIndex = 0;
int AggreFeatIndex = 0;
int FeaturePoseGradIndex = 0;
for (MatchedIterator = FeatMatched.begin(); MatchedIterator != FeatMatched.end(); MatchedIterator++)
{
if ( (*MatchedIterator).InMapFeat->isMature () ) {
// get position grandient, from observed predicted feature
(*MatchedIterator).Observed->getPoseGradient(Gg,&FeaturePoseGradIndex);
// set innovation vector
for (int i=0;i<(*MatchedIterator).InovVect.size();i++) {
gsl_matrix_set (Vk, InnovIndex, 0, (*MatchedIterator).InovVect[i] );
InnovIndex++;
}
// aggregated feature covariance matrix
(*MatchedIterator).Observed->getCovarianceDiagonalElems (QK,&AggreFeatIndex);
}
}
// transpose the matrix GT
gsl_matrix_transpose_memcpy(GgT, Gg);
/* Pose Correction of EKF
*
* S(k + 1) = Gg(k+1).R(k + 1|k).GgT(k+1) + Q(k + 1) (5.8)
* */
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Gg, CovPose, 0.0, mat2nx3);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, mat2nx3, GgT, 0.0, SK);
gsl_matrix_add(SK,QK);
printgslMatrix ( Gg, 2*Featincorrection , 3, "Gg");
printgslMatrix ( QK, 2*Featincorrection , 2*Featincorrection, "QK");
printgslMatrix ( SK, 2*Featincorrection , 2*Featincorrection, "SK");
/* matrix inversion Sk */
CalcInvMatrix(SKinv, SK, Featincorrection * 2 );
/* Kalman gain
* w(k + 1) = R(k + 1|k).GgT(k + 1).S^−1(k + 1) (5.11)
* */
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, CovPose, GgT, 0.0, mat3xn2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, mat3xn2, SKinv, 0.0, W);
// transpose the Gain
gsl_matrix_transpose_memcpy(WT, W);
/* Pose Correction eq.
* x(k + 1|k + 1) = x(k + 1|k) + w(k + 1).v(k + 1) (5.10)
* */
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, W, Vk, 0.0, mat3x1);
gsl_matrix_add(MeanPose,mat3x1);
/* Covariance Correction eq.
* R(k + 1|k + 1) = R(k + 1|k) − w(k + 1).S(k + 1).wT(k + 1) (5.12)
* */
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, W, SK, 0.0, mat3xn2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, mat3xn2, WT, 0.0, mat3x3);
gsl_matrix_sub(CovPose,mat3x3);
// free matrix elements
gsl_matrix_free(Vk);
gsl_matrix_free(Gg);
gsl_matrix_free(GgT);
gsl_matrix_free(W);
gsl_matrix_free(WT);
gsl_matrix_free(QK);
gsl_matrix_free(SK);
gsl_matrix_free(SKinv);
gsl_matrix_free(mat2nx3);
gsl_matrix_free(mat3xn2);
gsl_matrix_free(mat3x3);
gsl_matrix_free(mat3x1);
} //end update position
gsl_matrix* calc_Hs(double s, double tres, double* Qxx_m, double* Qyy_m, double* Qxy_m, double* Qyx_m, int kx, int ky) {
/*
*
*
* *H(s)
*I=eye(ky);
*expMat=mat_exp(-(s*I-Qyy)*tres);
*Hxx_s=Qxx+(Qxy*(s*I-Qyy)^-1)*(I-expMat)*Qyx;
*
*/
/* Initialise Qxx,Qxy,Qyx,Qyy */
gsl_matrix *Qxx=setup_matrix(Qxx_m,kx,kx);
gsl_matrix *Qxy=setup_matrix(Qxy_m,kx,ky);
gsl_matrix *Qyx=setup_matrix(Qyx_m,ky,kx);
gsl_matrix *Qyy=setup_matrix(Qyy_m,ky,ky);
gsl_matrix *ky_eye; //identitny matrix of dimension ky,ky
gsl_matrix *e_eye; //to hold the matrix exponential expMat
gsl_matrix *detected_tres; //matrix representing period in states y for tres followed by transition to x
gsl_matrix *seye2;
gsl_matrix *inv_sI_Qyy;
gsl_matrix *res;
gsl_matrix *res2;
gsl_matrix *res3;
gsl_matrix *eye3;
//printf("\tAllocate memory for the matrices\n");
//allocate memory for all the matrices
ky_eye=gsl_matrix_alloc(ky,ky);
e_eye=gsl_matrix_alloc(ky,ky);
seye2=gsl_matrix_alloc(ky,ky);
detected_tres=gsl_matrix_alloc(ky,ky);
inv_sI_Qyy=gsl_matrix_alloc(ky,ky);
res=gsl_matrix_alloc(kx,ky);
res2=gsl_matrix_alloc(ky,kx);
res3 = gsl_matrix_alloc(kx,kx);
eye3=gsl_matrix_alloc(ky,ky);
//printf("\tAllocated memory for the matrices\n");
gsl_matrix_set_identity(ky_eye);
//exp(-(s*I-Qyy)*tres)
//build from the identity matrix
gsl_matrix_memcpy (detected_tres,ky_eye);
gsl_matrix_scale(detected_tres,s);
gsl_matrix_sub(detected_tres,Qyy);
gsl_matrix_scale(detected_tres,-1);
gsl_matrix_scale(detected_tres,tres);
gsl_linalg_exponential_ss(detected_tres, e_eye, .01);
//printf("\tCalculated exp(-(s*I-Qyy)*tres)\n");
//(s*I-Qyy)
gsl_matrix_memcpy (seye2,ky_eye);
gsl_matrix_scale(seye2,s);
gsl_matrix_sub(seye2,Qyy);
//printf("\tCalculated s*I-Qyy\n");
//invert s*I-Qyy
int sc;
gsl_permutation * p = gsl_permutation_alloc (ky);
gsl_linalg_LU_decomp(seye2, p, &sc);
gsl_linalg_LU_invert(seye2, p, inv_sI_Qyy);
gsl_permutation_free(p);
//printf("\tInverted s*I-Qyy\n");
/*
for (int i=0; i<ky; i++){
for (int j=0; j<ky; j++){
mexPrintf("\t%f",gsl_matrix_get(inv_sI_Qyy,i,j));
}
mexPrintf("\n");
}
*/
//multiply Qxy * (s*I-Qyy)^-1
gsl_matrix_set_zero(res);
// res = (Qxy*(s*I-Qyy)^-1)
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
1.0, Qxy, inv_sI_Qyy,
0.0, res);
//printf("\tCalculated Qxy * (s*I-Qyy)^-1\n");
//res2 =(I-expMat)*Qyx;
gsl_matrix_set_zero(res2);
gsl_matrix_memcpy (eye3,ky_eye);
//printf("\tMemcpy (I-expMat)\n");
gsl_matrix_sub(eye3,e_eye);
//printf("\tSubtract (I-expMat)\n");
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
1.0, eye3, Qyx,
0.0, res2);
//res3 = (Qxy*(s*I-Qyy)^-1)*(I-expMat)*Qyx;
//res3 = res *res2
//res3 is the result we want to return
//printf("\t (I-expMat)*Qyx\n");
gsl_matrix_set_zero(res3);
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
1.0, res, res2,
0.0, res3);
gsl_matrix_add(res3,Qxx);
/*
for (int i=0; i<kx; i++){
for (int j=0; j<kx; j++){
mexPrintf("\t%f",gsl_matrix_get(res3,i,j));
}
mexPrintf("\n");
}
*/
//printf("\t Calced H(s)\n");
//cleanup
gsl_matrix_free(Qxx);
gsl_matrix_free(Qxy);
gsl_matrix_free(Qyx);
gsl_matrix_free(Qyy);
gsl_matrix_free(ky_eye);
gsl_matrix_free(e_eye);
gsl_matrix_free(detected_tres);
gsl_matrix_free(seye2);
gsl_matrix_free(inv_sI_Qyy);
gsl_matrix_free(res);
gsl_matrix_free(res2);
gsl_matrix_free(eye3);
//printf("\t Cleaned up H(s)\n");
return res3;
}
void KF_steady (int *dim, double *sy, double *sZ, double *sT, double *sH,
double *sR, double *sV, double *sQ, double *sa0, double *sP0,
double *mll,
std::vector<double> *v, std::vector<double> *f,
std::vector<double> *invf, std::vector<double> *vof,
gsl_matrix *K, std::vector<gsl_matrix*> *L,
double *tol, int *maxiter)
{
//int s, p = dim[1], r = dim[3];
int i, n = dim[0], m = dim[2], conv = 0, counter = 0;
//double Kisum, Kim1sum = 0.0;
mll[0] = 0.0;
// data and state space model matrices
gsl_vector_view Z = gsl_vector_view_array(sZ, m);
gsl_matrix_view T = gsl_matrix_view_array(sT, m, m);
gsl_matrix_view Q = gsl_matrix_view_array(sQ, m, m);
gsl_vector * a_pred = gsl_vector_alloc(m);
//gsl_matrix * P_pred = gsl_matrix_alloc(m, m);
// storage vectors and matrices
gsl_vector * Vm = gsl_vector_alloc(m);
gsl_vector * Vm_cp = gsl_vector_alloc(m);
gsl_vector * Vm_cp2 = gsl_vector_alloc(m);
gsl_matrix * Mmm = gsl_matrix_alloc(m, m);
gsl_vector_view a0 = gsl_vector_view_array(sa0, m);
gsl_vector * a_upd = gsl_vector_alloc(m);
gsl_vector_memcpy(a_upd, &a0.vector);
gsl_matrix_view P0 = gsl_matrix_view_array(sP0, m, m);
gsl_matrix * P_upd = gsl_matrix_alloc(m, m);
gsl_matrix_memcpy(P_upd, &P0.matrix);
gsl_vector_view K_irow, K_im1row, Kri;
gsl_matrix_view maux1, maux2;
// filtering recursions
for (i = 0; i < n; i++)
{
gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, a_upd, 0.0, a_pred);
if (conv == 0) {
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &T.matrix, P_upd,
0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, Mmm, &T.matrix,
0.0, P_upd);
gsl_matrix_add(P_upd, &Q.matrix);
}
gsl_blas_ddot(&Z.vector, a_pred, &v->at(i));
v->at(i) = sy[i] - v->at(i);
if (conv == 0) {
gsl_blas_dgemv(CblasNoTrans, 1.0, P_upd, &Z.vector, 0.0, Vm);
gsl_blas_ddot(&Z.vector, Vm, &f->at(i));
f->at(i) += *sH;
invf->at(i) = 1.0 / f->at(i);
} else {
f->at(i) = f->at(i-1);
invf->at(i) = invf->at(i-1);
}
gsl_vector_memcpy(Vm_cp, Vm);
gsl_vector_memcpy(Vm_cp2, Vm);
vof->at(i) = v->at(i) * invf->at(i);
mll[0] += 0.5 * (log(M_2PI * f->at(i)) + (v->at(i) * vof->at(i)));
if (conv == 0) {
maux1 = gsl_matrix_view_array(gsl_vector_ptr(Vm_cp, 0), m, 1);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &maux1.matrix,
&maux1.matrix, 0.0, Mmm);
gsl_matrix_scale(Mmm, invf->at(i));
gsl_matrix_sub(P_upd, Mmm);
}
gsl_vector_memcpy(a_upd, a_pred);
gsl_vector_scale(Vm_cp2, vof->at(i));
gsl_vector_add(a_upd, Vm_cp2);
K_irow = gsl_matrix_row(K, i);
if (conv == 0) {
gsl_vector_scale(Vm_cp, invf->at(i));
gsl_blas_dgemv(CblasNoTrans, 1.0, &T.matrix, Vm_cp, 0.0, &K_irow.vector);
} else {
K_im1row = gsl_matrix_row(K, i-1);
gsl_vector_memcpy(&K_irow.vector, &K_im1row.vector);
}
L[0].at(i) = gsl_matrix_alloc(m, m);
if (conv == 0) {
maux1 = gsl_matrix_view_array(gsl_vector_ptr(&K_irow.vector, 0), m, 1);
maux2 = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), 1, m);
gsl_matrix_memcpy(L[0].at(i), &T.matrix);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &maux1.matrix,
&maux2.matrix, 1.0, L[0].at(i));
} else {
gsl_matrix_memcpy(L[0].at(i), L[0].at(i-1));
}
// check if convergence to the steady state has been reached
if ((i > 0) & (conv == 0))
{
//if (i == 1)
//{
// Kim1sum = 0.0;
//}
Kri = gsl_matrix_row(K, i);
std::vector<double> vm((&Kri.vector)->data, (&Kri.vector)->data + m);
//Kisum = std::accumulate(vm.begin(), vm.end(), 0.0);
if (fabs(f->at(i) - f->at(i-1)) < *tol)
{
counter += 1;
}
//Kim1sum = Kisum;
if (counter == *maxiter) {
conv = 1;
dim[5] = i;
}
}
}
// deallocate memory
gsl_vector_free(a_pred);
gsl_vector_free(a_upd);
gsl_matrix_free(P_upd);
gsl_vector_free(Vm);
gsl_vector_free(Vm_cp);
gsl_vector_free(Vm_cp2);
gsl_matrix_free(Mmm);
}
int transient_simulation(LIST* list, gsl_matrix *matrix , gsl_matrix *c_matrix , gsl_vector *vector , gsl_vector *x, gsl_permutation* permutation)
{
LIST* v_i_list;
double fin_time = list->fin_time;
double curr_time = 0;
double time_step = list->time_step;
time_step = 0.1;
int flag;
gsl_vector *prev_vector;
gsl_vector *temp_vector;
gsl_vector *e_t;
gsl_vector *e_t_minus_one;
gsl_vector *prev_x;
gsl_matrix *right_matrix;
gsl_matrix *tmp_matrix;
gsl_matrix *curr_matrix;
int i;
// sparse simulation
int vector_size;
char method;
int sign;
sparse_matrix* sp_matrix;
sparse_vector* sp_vector;
sparse_vector* sp_x;
gsl_vector* x_sparse;
gsl_vector* vector_sparse;
gsl_vector* vector_gsl;
if(!list->sparse )
{
flag = create_mna(list, &matrix, &vector, 1 ,&c_matrix);
//print_matrix_gsl(matrix);
if(!flag ) {
printf("Error creating mna system\n");
return -1;
}
v_i_list = create_source_list(list);
tmp_matrix = gsl_matrix_calloc(matrix->size1 , matrix->size1);
curr_matrix = gsl_matrix_calloc(matrix->size1 , matrix->size1);
right_matrix = gsl_matrix_calloc(matrix->size1,matrix->size2);
if( !tmp_matrix || !right_matrix)
return 0;
gsl_matrix_memcpy(right_matrix,matrix);
gsl_matrix_memcpy(tmp_matrix,matrix);
gsl_matrix_memcpy(curr_matrix,matrix);
x = gsl_vector_calloc(matrix->size1);
if( !x ) {
printf("X vector : no memory\n");
exit(1);
}
if(list->transient_sim == METHOD_TR)
{
gsl_matrix_scale(c_matrix, (2/time_step));
gsl_matrix_add(tmp_matrix,c_matrix);
gsl_matrix_sub(right_matrix,c_matrix);
} else
{
gsl_matrix_scale (c_matrix, 1/time_step);
gsl_matrix_add(tmp_matrix,c_matrix);
gsl_matrix_memcpy(right_matrix , c_matrix);
}
x = gsl_vector_calloc(matrix->size1);
if( !x ) {
printf("X vector : no memory\n");
exit(1);
}
} else
{
method = list->solving_method;
sp_matrix = (sparse_matrix*)create_mna_sparse( list , &sp_vector , &vector_size);
if( !sp_matrix ) {
fprintf(stderr, "Error creating MNA matrix \n");
exit(1);
}
/* conversion of a double into a gsl */
x_sparse = gsl_vector_calloc(sp_matrix->n);
vector_sparse = gsl_vector_calloc(sp_matrix->n);
double_vector_to_gsl_vector(vector_sparse,sp_vector,vector_size);
sp_x = (sparse_vector*) malloc( vector_size * sizeof(sparse_vector));
}
/*
printf("matrix:\n");
print_matrix_gsl(matrix);
printf("Tmp Matrix:\n");
print_matrix_gsl(tmp_matrix);
printf("right_matrix:\n");
print_matrix_gsl(right_matrix);
printf("c_matrix:\n");
print_matrix_gsl(c_matrix);
*/
prev_vector = gsl_vector_calloc(vector->size);
temp_vector = gsl_vector_calloc(vector->size);
if( !prev_vector || !temp_vector) {
printf("Couldn't allocate memory for prev_vector & temp_vector");
return 0;
}
prev_x = gsl_vector_calloc(x->size);
if( !prev_x ) {
printf("Couldn't allocate memory for prev_x");
return 0;
}
e_t_minus_one = gsl_vector_calloc(vector->size);
e_t = gsl_vector_calloc(vector->size);
if( !e_t || !e_t_minus_one) {
printf("Couldn't allocate memory for e(t) & e(t-1)");
return 0;
}
printf("TRANSIENT: Solving Method = %d\n",list->solving_method);
printf("fin_time: %lf\n",fin_time);
for(curr_time = (-1)*time_step; curr_time <= 3; curr_time += time_step)
{
if(curr_time == 0)
{
gsl_vector_memcpy(e_t_minus_one,vector); //dc initialize
gsl_matrix_memcpy(matrix,tmp_matrix);
}
if(curr_time >= 0)
{
// x(t-1)
gsl_vector_memcpy(prev_x,x);
//print_vector_gsl(temp_vector);
// right_matrix * x(t-1)
lh_matrix_vector_mul( prev_x, right_matrix , temp_vector , NON_TRANSP);
//print_vector_gsl(temp_vector);
// calculate e(t)
e_t = calc_right_hand_vect(v_i_list , e_t_minus_one ,curr_time);
/*gsl_vector_memcpy(prev_vector,e_t);
gsl_vector_add(prev_vector,e_t_minus_one);
printf("e(t) + e(t-1) = \n");
print_vector_gsl(prev_vector); */
// get ready for next math operations
gsl_vector_memcpy(prev_vector,e_t_minus_one);
gsl_vector_memcpy(vector,e_t);
if(list->transient_sim == METHOD_TR)
{
// e(t-1) - right_matrix*x(t-1)
gsl_vector_sub(prev_vector, temp_vector);
// e(t) + e(t-1) - right_matrix*x(t-1)
gsl_vector_add(vector,prev_vector);
printf("b = \n");
print_vector_gsl(vector);
// e_t_minus_one = e_t for the next iteration
gsl_vector_memcpy(e_t_minus_one,e_t);
}
LIST_NODE* curr;
}
for(i = 0; i < x->size; i++)
gsl_vector_set(x,i,0);
if ( !list->sparse ) {
/* Cholesky or LU */
if( list->solving_method == METHOD_LU || list->solving_method == METHOD_CHOLESKY ) {
gsl_matrix_memcpy(curr_matrix , matrix);
decomposition(matrix,&permutation,&sign,list->solving_method);
if(list->dc_sweep.node != NULL)
{
dc_sweep(*list,matrix,vector,x,permutation,list->solving_method);
}
else
{
int array_size = 1;
solve(matrix,vector,x,permutation,list->solving_method);
print_vector_gsl(x);
if(list->plot == PLOT_ON)
{
gsl_vector ** plot_array;
plot_array = plot_create_vector( array_size , x->size);
if(plot_array == NULL)
{
perror("Error while allocating the ploting array\n");
exit(0);
}
plot_set_vector_index(plot_array ,x ,0);
if ( list->solving_method == METHOD_LU )
plot_to_file(list->hashtable,plot_array,array_size,"results_plot_file_lu.txt");
else
plot_to_file(list->hashtable,plot_array,array_size,"results_plot_file_chol.txt");
}
}
gsl_matrix_memcpy(matrix , curr_matrix);
}
else if ( list->solving_method == METHOD_CG ) {
if( list->dc_sweep.node != NULL ) {
dc_sweep(*list,matrix,vector,x,permutation,list->solving_method);
}
else {
iter_solve_cg( matrix , vector , x);
gsl_vector ** plot_array;
plot_array = plot_create_vector( 1 , x->size);
if(plot_array == NULL)
{
perror("Error while allocating the ploting array\n");
exit(0);
}
plot_set_vector_index(plot_array ,x ,0);
plot_to_file(list->hashtable,plot_array,1 ,"results_plot_file_cg.txt");
}
}
else if( list->solving_method == METHOD_BICG) {
if( list->dc_sweep.node != NULL ) {
dc_sweep(*list,matrix,vector,x,permutation,list->solving_method);
}
else {
iter_solve_bicg( matrix , vector , x);
gsl_vector ** plot_array;
plot_array = plot_create_vector( 1 , x->size);
if(plot_array == NULL)
{
perror("Error while allocating the ploting array\n");
exit(0);
}
plot_set_vector_index(plot_array ,x ,0);
plot_to_file(list->hashtable,plot_array,1 ,"results_plot_file_bicg.txt");
}
}
}
else {
if( method == METHOD_LU_SPARSE ) {
if( !sparse_solve_LU( sp_matrix,sp_vector,sp_x,vector_size) ) {
fprintf(stderr, "Solving Method Sparse LU failed\n" );
}
gsl_vector ** plot_array;
plot_array = plot_create_vector( 1 , vector_size);
if(plot_array == NULL)
{
perror("Error while allocating the ploting array\n");
exit(0);
}
vector_gsl = gsl_vector_calloc(vector_size);
double_vector_to_gsl_vector(vector_gsl,sp_x,vector_size);
plot_set_vector_index(plot_array ,vector_gsl ,0);
plot_to_file(list->hashtable,plot_array,1 ,"results_plot_file_lu_sparse.txt");
}
else if( method == METHOD_CHOLESKY_SPARSE ) {
if( !sparse_solve_cholesky( sp_matrix,sp_vector,sp_x,vector_size) ) {
fprintf(stderr, "Solving Method Sparse Cholesky failed\n" );
}
}
else if ( method == METHOD_CG_SPARSE ) {
if( !sparse_solve_cg( sp_matrix,vector_sparse,x_sparse) ) {
fprintf(stderr, "Solving Method Sparse CG failed\n" );
}
gsl_vector ** plot_array;
plot_array = plot_create_vector( 1 , x_sparse->size);
if(plot_array == NULL)
{
perror("Error while allocating the ploting array\n");
exit(0);
}
plot_set_vector_index(plot_array ,x_sparse ,0);
plot_to_file(list->hashtable,plot_array,1 ,"results_plot_file_sparse_cg.txt");
}
else if( method == METHOD_BICG_SPARSE ) {
if( !sparse_solve_bicg( sp_matrix, vector_sparse, x_sparse) ) {
fprintf(stderr, "Solving Method Sparse BiCG failed\n" );
}
gsl_vector ** plot_array;
plot_array = plot_create_vector( 1 , x_sparse->size);
if(plot_array == NULL)
{
perror("Error while allocating the ploting array\n");
exit(0);
}
plot_set_vector_index(plot_array ,x_sparse ,0);
plot_to_file(list->hashtable,plot_array,1 ,"results_plot_file_sparse_bicg.txt");
}
else {
fprintf(stderr, "Solving method not specified\n");
}
}
}
/* clean up before exit */
if(list->sparse)
cs_spfree(sp_matrix);
free(vector);
free(x);
return 1;
}
AnovaTest::AnovaTest(mv_Method *mm, gsl_matrix *Y, gsl_matrix *X, gsl_matrix *isXvarIn):mmRef(mm), Yref(Y), Xref(X), inRef(isXvarIn)
{
unsigned int hid, aid;
unsigned int i, j, count;
nModels=inRef->size1, nParam=Xref->size2;
nRows=Yref->size1, nVars=Yref->size2;
// printf("initialize public variables: stats\n");
multstat=(double *)malloc((nModels-1)*sizeof(double));
Pmultstat = (double *)malloc((nModels-1)*sizeof(double));
for (j=0; j<nModels-1; j++) *(Pmultstat+j)=0.0;
dfDiff = (unsigned int *)malloc((nModels-1)*sizeof(unsigned int));
statj = gsl_matrix_alloc(nModels-1, nVars);
Pstatj = gsl_matrix_alloc(nModels-1, nVars);
gsl_matrix_set_zero(Pstatj);
bStatj = gsl_vector_alloc(nVars);
Hats = (mv_mat *)malloc(nModels*sizeof(mv_mat));
sortid = (gsl_permutation **)malloc((nModels-1)*sizeof(gsl_permutation *));
for (i=0; i<nModels; i++ ) {
// Hats[i]
Hats[i].mat=gsl_matrix_alloc(nRows, nRows);
Hats[i].SS=gsl_matrix_alloc(nVars, nVars);
Hats[i].R=gsl_matrix_alloc(nVars, nVars);
Hats[i].Res=gsl_matrix_alloc(nRows, nVars);
Hats[i].Y = gsl_matrix_alloc(nRows, nVars);
Hats[i].sd = gsl_vector_alloc(nVars);
count = 0;
for (j=0; j<nParam; j++){
count+=(unsigned int)gsl_matrix_get(inRef, i, j);
}
// printf("count=%d \n", count);
Hats[i].X = gsl_matrix_alloc(nRows, count);
Hats[i].Coef=gsl_matrix_alloc(count, nVars);
gsl_vector_view refi=gsl_matrix_row(inRef, i);
subX(Xref, &refi.vector, Hats[i].X);
calcSS(Yref, &(Hats[i]), mmRef);
// displaymatrix(Hats[i].SS, "SS");
}
for (i=1; i<nModels; i++) {
hid = i; aid = i-1;
if ( mmRef->resamp != CASEBOOT ) {
// fit = Y- resi
gsl_matrix_memcpy (Hats[i].Y, Yref);
gsl_matrix_sub (Hats[i].Y, Hats[i].Res);
}
gsl_vector_view statij = gsl_matrix_row(statj, aid);
testStatCalc(&(Hats[hid]), &(Hats[aid]), mmRef, TRUE, (multstat+aid), &statij.vector);
dfDiff[aid] = Hats[aid].X->size2-Hats[hid].X->size2;
// sortid
sortid[aid] = gsl_permutation_alloc(nVars);
gsl_sort_vector_index (sortid[aid], &statij.vector);
// rearrange sortid in descending order
gsl_permutation_reverse (sortid[aid]);
}
// initialize resampling indices
// getBootID(); done in R
bootID = NULL;
// Initialize GSL rnd environment variables
const gsl_rng_type *T;
gsl_rng_env_setup();
T = gsl_rng_default;
// an mt19937 generator with a seed of 0
rnd = gsl_rng_alloc(T);
if (mmRef->reprand!=TRUE){
struct timeval tv; // seed generation based on time
gettimeofday(&tv, 0);
unsigned long mySeed=tv.tv_sec + tv.tv_usec;
gsl_rng_set(rnd, mySeed); // reset seed
}
// printf("Anova test initialized.\n");
}