static void
take_step (const gsl_vector * x, const gsl_vector * p,
double step, double lambda, gsl_vector * x1, gsl_vector * dx)
{
gsl_vector_set_zero (dx);
gsl_blas_daxpy (-step * lambda, p, dx);
gsl_vector_memcpy (x1, x);
gsl_blas_daxpy (1.0, dx, x1);
}
static int
cod_householder_mh(const double tau, const gsl_vector * v, gsl_matrix * A,
gsl_vector * work)
{
if (tau == 0)
{
return GSL_SUCCESS; /* H = I */
}
else
{
const size_t M = A->size1;
const size_t N = A->size2;
const size_t L = v->size;
gsl_vector_view A1 = gsl_matrix_subcolumn(A, 0, 0, M);
gsl_matrix_view C = gsl_matrix_submatrix(A, 0, N - L, M, L);
/* work(1:M) = A(1:M,1) */
gsl_vector_memcpy(work, &A1.vector);
/* work(1:M) = work(1:M) + A(1:M,M+1:N) * v(1:N-M) */
gsl_blas_dgemv(CblasNoTrans, 1.0, &C.matrix, v, 1.0, work);
/* A(1:M,1) = A(1:M,1) - tau * work(1:M) */
gsl_blas_daxpy(-tau, work, &A1.vector);
/* A(1:M,M+1:N) = A(1:M,M+1:N) - tau * work(1:M) * v(1:N-M)' */
gsl_blas_dger(-tau, work, v, &C.matrix);
return GSL_SUCCESS;
}
}
static VALUE rb_gsl_blas_daxpy2(int argc, VALUE *argv, VALUE obj)
{
double a;
gsl_vector *x = NULL, *y = NULL, *y2 = NULL;
switch (TYPE(obj)) {
case T_MODULE:
case T_CLASS:
case T_OBJECT:
get_vector2(argc-1, argv+1, obj, &x, &y);
Need_Float(argv[0]);
// a = RFLOAT(argv[0])->value;
a = NUM2DBL(argv[0]);
break;
default:
Data_Get_Struct(obj, gsl_vector, x);
if (argc != 2) rb_raise(rb_eArgError, "wrong number of arguments (%d for 2)",
argc);
Need_Float(argv[0]);
CHECK_VECTOR(argv[1]);
// a = RFLOAT(argv[0])->value;
a = NUM2DBL(argv[0]);
Data_Get_Struct(argv[1], gsl_vector, y);
break;
}
y2 = gsl_vector_alloc(y->size);
gsl_vector_memcpy(y2, y);
gsl_blas_daxpy(a, x, y2);
return Data_Wrap_Struct(cgsl_vector, 0, gsl_vector_free, y2);
}
static double
nmsimplex_size (nmsimplex_state_t * state)
{
/* calculates simplex size as average sum of length of vectors
from simplex center to corner points:
( sum ( || y - y_middlepoint || ) ) / n
*/
gsl_vector *s = state->ws1;
gsl_vector *mp = state->ws2;
gsl_matrix *x1 = state->x1;
size_t i;
double ss = 0.0;
/* Calculate middle point */
nmsimplex_calc_center (state, mp);
for (i = 0; i < x1->size1; i++)
{
gsl_matrix_get_row (s, x1, i);
gsl_blas_daxpy (-1.0, mp, s);
ss += gsl_blas_dnrm2 (s);
}
return ss / (double) (x1->size1);
}
int
gsl_linalg_householder_hv (double tau, const gsl_vector * v, gsl_vector * w)
{
/* applies a householder transformation v to vector w */
const size_t N = v->size;
if (tau == 0)
return GSL_SUCCESS ;
{
/* compute d = v'w */
double d0 = gsl_vector_get(w,0);
double d1, d;
gsl_vector_const_view v1 = gsl_vector_const_subvector(v, 1, N-1);
gsl_vector_view w1 = gsl_vector_subvector(w, 1, N-1);
gsl_blas_ddot (&v1.vector, &w1.vector, &d1);
d = d0 + d1;
/* compute w = w - tau (v) (v'w) */
{
double w0 = gsl_vector_get (w,0);
gsl_vector_set (w, 0, w0 - tau * d);
}
gsl_blas_daxpy (-tau * d, &v1.vector, &w1.vector);
}
return GSL_SUCCESS;
}
static void moveGN( const gsl_matrix *Vt, const gsl_vector *sig2, const gsl_vector *ufuncsig,
double lambda2, gsl_vector * dx, int k, gsl_vector * scaling ) {
gsl_vector_set_zero(dx);
double threshold = gsl_vector_get(sig2, 0) * Vt->size2 * DBL_EPSILON * 100;
size_t i;
for (i = 0; (i<sig2->size) &&(gsl_vector_get(sig2, i) >= threshold); i++) {
gsl_vector VtRow = gsl_matrix_const_row(Vt, i).vector;
gsl_blas_daxpy(gsl_vector_get(ufuncsig, i) /
(gsl_vector_get(sig2, i) * gsl_vector_get(sig2, i) + lambda2), &VtRow, dx);
}
/* if (i >= k) {
PRINTF("Pseudoinverse threshold exceeded.\n");
PRINTF("Threshold: %g, i: %d, last: %g, next: %g\n",
threshold, i, gsl_vector_get(sig2, i-1), gsl_vector_get(sig2, i));
}*/
if (scaling != NULL) {
gsl_vector_mul(dx, scaling);
}
}
static VALUE rb_gsl_blas_daxpy(int argc, VALUE *argv, VALUE obj)
{
double a;
gsl_vector *x = NULL, *y = NULL;
switch (TYPE(obj)) {
case T_MODULE:
case T_CLASS:
case T_OBJECT:
get_vector2(argc-1, argv+1, obj, &x, &y);
Need_Float(argv[0]);
// a = RFLOAT(argv[0])->value;
a = NUM2DBL(argv[0]);
break;
default:
Data_Get_Struct(obj, gsl_vector, x);
if (argc != 2) rb_raise(rb_eArgError, "wrong number of arguments (%d for 2)",
argc);
Need_Float(argv[0]);
// a = RFLOAT(argv[0])->value;
a = NUM2DBL(argv[0]);
Data_Get_Vector(argv[1], y);
break;
}
gsl_blas_daxpy(a, x, y);
return argv[argc-1];
}
static int
cod_householder_hv(const double tau, const gsl_vector * v, gsl_vector * w)
{
if (tau == 0)
{
return GSL_SUCCESS; /* H = I */
}
else
{
const size_t M = w->size;
const size_t L = v->size;
double w0 = gsl_vector_get(w, 0);
gsl_vector_view w1 = gsl_vector_subvector(w, M - L, L);
double d1, d;
/* d1 := v . w(M-L:M) */
gsl_blas_ddot(v, &w1.vector, &d1);
/* d := w(1) + v . w(M-L:M) */
d = w0 + d1;
/* w(1) = w(1) - tau * d */
gsl_vector_set(w, 0, w0 - tau * d);
/* w(M-L:M) = w(M-L:M) - tau * d * v */
gsl_blas_daxpy(-tau * d, v, &w1.vector);
return GSL_SUCCESS;
}
}
/*
Computes covariance using the renormalization above and adds it to
an existing matrix.
*/
void MultinomialCovariance(double alpha,
const gsl_vector* v,
gsl_matrix* m) {
double scale = gsl_blas_dsum(v);
gsl_blas_dger(-alpha / scale, v, v, m);
gsl_vector_view diag = gsl_matrix_diagonal(m);
gsl_blas_daxpy(alpha, v, &diag.vector);
}
int
gsl_eigen_gensymm_standardize(gsl_matrix *A, const gsl_matrix *B)
{
const size_t N = A->size1;
size_t i;
double a, b, c;
for (i = 0; i < N; ++i)
{
/* update lower triangle of A(i:n, i:n) */
a = gsl_matrix_get(A, i, i);
b = gsl_matrix_get(B, i, i);
a /= b * b;
gsl_matrix_set(A, i, i, a);
if (i < N - 1)
{
gsl_vector_view ai = gsl_matrix_subcolumn(A, i, i + 1, N - i - 1);
gsl_matrix_view ma =
gsl_matrix_submatrix(A, i + 1, i + 1, N - i - 1, N - i - 1);
gsl_vector_const_view bi =
gsl_matrix_const_subcolumn(B, i, i + 1, N - i - 1);
gsl_matrix_const_view mb =
gsl_matrix_const_submatrix(B, i + 1, i + 1, N - i - 1, N - i - 1);
gsl_blas_dscal(1.0 / b, &ai.vector);
c = -0.5 * a;
gsl_blas_daxpy(c, &bi.vector, &ai.vector);
gsl_blas_dsyr2(CblasLower, -1.0, &ai.vector, &bi.vector, &ma.matrix);
gsl_blas_daxpy(c, &bi.vector, &ai.vector);
gsl_blas_dtrsv(CblasLower,
CblasNoTrans,
CblasNonUnit,
&mb.matrix,
&ai.vector);
}
}
return GSL_SUCCESS;
} /* gsl_eigen_gensymm_standardize() */
int
gsl_linalg_symmtd_decomp (gsl_matrix * A, gsl_vector * tau)
{
if (A->size1 != A->size2)
{
GSL_ERROR ("symmetric tridiagonal decomposition requires square matrix",
GSL_ENOTSQR);
}
else if (tau->size + 1 != A->size1)
{
GSL_ERROR ("size of tau must be (matrix size - 1)", GSL_EBADLEN);
}
else
{
const size_t N = A->size1;
size_t i;
for (i = 0 ; i < N - 2; i++)
{
gsl_vector_view c = gsl_matrix_column (A, i);
gsl_vector_view v = gsl_vector_subvector (&c.vector, i + 1, N - (i + 1));
double tau_i = gsl_linalg_householder_transform (&v.vector);
/* Apply the transformation H^T A H to the remaining columns */
if (tau_i != 0.0)
{
gsl_matrix_view m = gsl_matrix_submatrix (A, i + 1, i + 1,
N - (i+1), N - (i+1));
double ei = gsl_vector_get(&v.vector, 0);
gsl_vector_view x = gsl_vector_subvector (tau, i, N-(i+1));
gsl_vector_set (&v.vector, 0, 1.0);
/* x = tau * A * v */
gsl_blas_dsymv (CblasLower, tau_i, &m.matrix, &v.vector, 0.0, &x.vector);
/* w = x - (1/2) tau * (x' * v) * v */
{
double xv, alpha;
gsl_blas_ddot(&x.vector, &v.vector, &xv);
alpha = - (tau_i / 2.0) * xv;
gsl_blas_daxpy(alpha, &v.vector, &x.vector);
}
/* apply the transformation A = A - v w' - w v' */
gsl_blas_dsyr2(CblasLower, -1.0, &v.vector, &x.vector, &m.matrix);
gsl_vector_set (&v.vector, 0, ei);
}
gsl_vector_set (tau, i, tau_i);
}
return GSL_SUCCESS;
}
}
//------------------------------------------------------------------------------
double
calc_beta_pr (double g0norm, double g1norm,
gsl_vector *gradient, gsl_vector *g0)
{
double g0g1, beta;
gsl_blas_daxpy (-1.0, gradient, g0); // g0' = g0 - g1
gsl_blas_ddot(g0, gradient, &g0g1); // g1g0 = (g0-g1).g1
beta = g0g1 / (g0norm*g0norm); // beta = -((g1 - g0).g1)/(g0.g0)
return (beta);
}
static void moveto(double alpha, wrapper_t * w)
{
if (alpha == w->x_cache_key) { /* using previously cached position */
return;
}
/*
* set x_alpha = x + alpha * p
*/
gsl_vector_memcpy(w->x_alpha, w->x);
gsl_blas_daxpy(alpha, w->p, w->x_alpha);
w->x_cache_key = alpha;
}
tnn_error tnn_module_fprop_bias(tnn_module *m){
//Routine check
if(m->t != TNN_MODULE_TYPE_BIAS){
return TNN_ERROR_MODULE_MISTYPE;
}
if(m->input->valid != true || m->output->valid != true || m->w.valid != true){
return TNN_ERROR_STATE_INVALID;
}
//fprop to output
TNN_MACRO_GSLTEST(gsl_blas_dcopy(&m->input->x, &m->output->x));
TNN_MACRO_GSLTEST(gsl_blas_daxpy(1.0, &m->w.x, &m->output->x));
return TNN_ERROR_SUCCESS;
}
tnn_error tnn_loss_bprop_euclidean(tnn_loss *l){
//Routine check
if(l->t != TNN_LOSS_TYPE_EUCLIDEAN){
return TNN_ERROR_LOSS_MISTYPE;
}
if(l->input1->valid != true || l->input2->valid != true || l->output->valid != true){
return TNN_ERROR_STATE_INVALID;
}
//bprop to input1 and input2 dx = dl 2 (x-y); dy = dl 2 (y-x)
TNN_MACRO_GSLTEST(gsl_blas_dcopy(&l->input1->x, &l->input1->dx));
TNN_MACRO_GSLTEST(gsl_blas_daxpy(-1.0, &l->input2->x, &l->input1->dx));
gsl_blas_dscal(2.0*gsl_vector_get(&l->output->dx, 0), &l->input1->dx);
TNN_MACRO_GSLTEST(gsl_blas_dcopy(&l->input1->dx, &l->input2->dx));
gsl_blas_dscal(-1.0, &l->input2->dx);
return TNN_ERROR_SUCCESS;
}
//Learn one sample using naive stochastic gradient descent
tnn_error tnn_trainer_class_learn_nsgd(tnn_trainer_class *t, gsl_vector *input, size_t label){
tnn_error ret;
tnn_state *sin;
tnn_param *p;
gsl_vector_view lb;
//Routine check
if(t->t != TNN_TRAINER_CLASS_TYPE_NSGD){
return TNN_ERROR_TRAINER_CLASS_MISTYPE;
}
//Check the input and label
TNN_MACRO_ERRORTEST(tnn_machine_get_sin(&t->m, &sin),ret);
if(label >= t->lset->size1 || input->size != sin->size){
return TNN_ERROR_STATE_INCOMP;
}
lb = gsl_matrix_row(t->lset, label);
//Set the loss output dx to be 1
gsl_vector_set(&t->l.output->dx, 0, 1.0);
//Copy the data into the input/label and do forward and backward propagation
TNN_MACRO_GSLTEST(gsl_blas_dcopy(input, &sin->x));
TNN_MACRO_GSLTEST(gsl_blas_dcopy(&lb.vector, &t->label->x));
TNN_MACRO_ERRORTEST(tnn_machine_fprop(&t->m), ret);
TNN_MACRO_ERRORTEST(tnn_loss_fprop(&t->l), ret);
TNN_MACRO_ERRORTEST(tnn_loss_bprop(&t->l), ret);
TNN_MACRO_ERRORTEST(tnn_machine_bprop(&t->m), ret);
//Compute the accumulated regularization paramter
TNN_MACRO_ERRORTEST(tnn_machine_get_param(&t->m, &p), ret);
TNN_MACRO_ERRORTEST(tnn_reg_addd(&t->r, p->x, p->dx, t->lambda), ret);
//Compute the parameter update
TNN_MACRO_GSLTEST(gsl_blas_daxpy(-((tnn_trainer_class_nsgd*)t->c)->eta, p->dx, p->x));
//Set the titer parameter
((tnn_trainer_class_nsgd*)t->c)->titer = 1;
return TNN_ERROR_SUCCESS;
}
tnn_error tnn_module_fprop_sum(tnn_module *m){
tnn_state **t;
//Routine check
if(m->t != TNN_MODULE_TYPE_SUM){
return TNN_ERROR_MODULE_MISTYPE;
}
if(m->input->valid != true || m->output->valid != true){
return TNN_ERROR_STATE_INVALID;
}
//fprop to output
TNN_MACRO_GSLTEST(gsl_blas_dscal(0.0, &m->output->x));
for(t = (tnn_state **)utarray_front(((tnn_module_sum*)m->c)->sarray);
t != NULL;
t = (tnn_state **)utarray_next(((tnn_module_sum*)m->c)->sarray, t)){
TNN_MACRO_GSLTEST(gsl_blas_daxpy(1.0, &(*t)->x, &m->output->x));
}
return TNN_ERROR_SUCCESS;
}
//Add the derivatives of the regularizer to the vector d
tnn_error tnn_reg_addd(tnn_reg *r, gsl_vector *w, gsl_vector *d, double lambda){
tnn_error ret;
gsl_vector *regd;
if(r->d != NULL){
regd = gsl_vector_alloc(d->size);
if(regd == NULL){
return TNN_ERROR_GSL;
}
//Check whether the execution is successful
if((ret = (*r->d)(r, w, regd)) != TNN_ERROR_SUCCESS){
gsl_vector_free(regd);
return ret;
}
if(gsl_blas_daxpy(lambda, regd, d) != 0){
gsl_vector_free(regd);
return TNN_ERROR_GSL;
}
gsl_vector_free(regd);
return TNN_ERROR_SUCCESS;
}
return TNN_ERROR_REG_FUNCNDEF;
}
tnn_error tnn_loss_fprop_euclidean(tnn_loss *l){
gsl_vector *diff;
double loss;
//Routine check
if(l->t != TNN_LOSS_TYPE_EUCLIDEAN){
return TNN_ERROR_LOSS_MISTYPE;
}
if(l->input1->valid != true || l->input2->valid != true || l->output->valid != true){
return TNN_ERROR_STATE_INVALID;
}
//Do the forward propagation
if((diff = gsl_vector_alloc(l->input1->size)) == NULL){
return TNN_ERROR_GSL;
}
TNN_MACRO_GSLTEST(gsl_blas_dcopy(&l->input1->x, diff));
TNN_MACRO_GSLTEST(gsl_blas_daxpy(-1.0, &l->input2->x, diff));
loss = gsl_blas_dnrm2(diff);
gsl_vector_set(&l->output->x, 0, loss*loss);
gsl_vector_free(diff);
return TNN_ERROR_SUCCESS;
}
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);
}
int
gsl_linalg_SV_decomp_mod (gsl_matrix * A,
gsl_matrix * X,
gsl_matrix * V, gsl_vector * S, gsl_vector * work)
{
size_t i, j;
const size_t M = A->size1;
const size_t N = A->size2;
if (M < N)
{
GSL_ERROR ("svd of MxN matrix, M<N, is not implemented", GSL_EUNIMPL);
}
else if (V->size1 != N)
{
GSL_ERROR ("square matrix V must match second dimension of matrix A",
GSL_EBADLEN);
}
else if (V->size1 != V->size2)
{
GSL_ERROR ("matrix V must be square", GSL_ENOTSQR);
}
else if (X->size1 != N)
{
GSL_ERROR ("square matrix X must match second dimension of matrix A",
GSL_EBADLEN);
}
else if (X->size1 != X->size2)
{
GSL_ERROR ("matrix X must be square", GSL_ENOTSQR);
}
else if (S->size != N)
{
GSL_ERROR ("length of vector S must match second dimension of matrix A",
GSL_EBADLEN);
}
else if (work->size != N)
{
GSL_ERROR ("length of workspace must match second dimension of matrix A",
GSL_EBADLEN);
}
if (N == 1)
{
gsl_vector_view column = gsl_matrix_column (A, 0);
double norm = gsl_blas_dnrm2 (&column.vector);
gsl_vector_set (S, 0, norm);
gsl_matrix_set (V, 0, 0, 1.0);
if (norm != 0.0)
{
gsl_blas_dscal (1.0/norm, &column.vector);
}
return GSL_SUCCESS;
}
/* Convert A into an upper triangular matrix R */
for (i = 0; i < N; i++)
{
gsl_vector_view c = gsl_matrix_column (A, i);
gsl_vector_view v = gsl_vector_subvector (&c.vector, i, M - i);
double tau_i = gsl_linalg_householder_transform (&v.vector);
/* Apply the transformation to the remaining columns */
if (i + 1 < N)
{
gsl_matrix_view m =
gsl_matrix_submatrix (A, i, i + 1, M - i, N - (i + 1));
gsl_linalg_householder_hm (tau_i, &v.vector, &m.matrix);
}
gsl_vector_set (S, i, tau_i);
}
/* Copy the upper triangular part of A into X */
for (i = 0; i < N; i++)
{
for (j = 0; j < i; j++)
{
gsl_matrix_set (X, i, j, 0.0);
}
{
double Aii = gsl_matrix_get (A, i, i);
gsl_matrix_set (X, i, i, Aii);
}
for (j = i + 1; j < N; j++)
{
double Aij = gsl_matrix_get (A, i, j);
gsl_matrix_set (X, i, j, Aij);
}
}
/* Convert A into an orthogonal matrix L */
for (j = N; j-- > 0;)
{
/* Householder column transformation to accumulate L */
double tj = gsl_vector_get (S, j);
gsl_matrix_view m = gsl_matrix_submatrix (A, j, j, M - j, N - j);
gsl_linalg_householder_hm1 (tj, &m.matrix);
}
/* unpack R into X V S */
gsl_linalg_SV_decomp (X, V, S, work);
/* Multiply L by X, to obtain U = L X, stored in U */
{
gsl_vector_view sum = gsl_vector_subvector (work, 0, N);
for (i = 0; i < M; i++)
{
gsl_vector_view L_i = gsl_matrix_row (A, i);
gsl_vector_set_zero (&sum.vector);
for (j = 0; j < N; j++)
{
double Lij = gsl_vector_get (&L_i.vector, j);
gsl_vector_view X_j = gsl_matrix_row (X, j);
gsl_blas_daxpy (Lij, &X_j.vector, &sum.vector);
}
gsl_vector_memcpy (&L_i.vector, &sum.vector);
}
}
return GSL_SUCCESS;
}
static int vector_bfgs3_iterate(void *vstate, gsl_multimin_function_fdf * fdf, gsl_vector * x, double *f, gsl_vector * gradient, gsl_vector * dx)
{
vector_bfgs3_state_t *state = (vector_bfgs3_state_t *) vstate;
double alpha = 0.0, alpha1;
gsl_vector *x0 = state->x0;
gsl_vector *g0 = state->g0;
gsl_vector *p = state->p;
double g0norm = state->g0norm;
double pnorm = state->pnorm;
double delta_f = state->delta_f;
double pg, dir;
int status;
double f0 = *f;
if (pnorm == 0.0 || g0norm == 0.0 || state->fp0 == 0) {
gsl_vector_set_zero(dx);
return GSL_ENOPROG;
}
if (delta_f < 0) {
double del = GSL_MAX_DBL(-delta_f, 10 * GSL_DBL_EPSILON * fabs(f0));
alpha1 = GSL_MIN_DBL(1.0, 2.0 * del / (-state->fp0));
} else {
alpha1 = fabs(state->step);
}
/*
* line minimisation, with cubic interpolation (order = 3)
*/
if (debug)
printf("...call minimize()\n");
status = minimize(&state->wrap.fdf_linear, state->rho, state->sigma, state->tau1, state->tau2, state->tau3, state->order, alpha1, &alpha);
if (debug)
printf("...end minimize()\n");
if (status != GSL_SUCCESS) {
update_position(&(state->wrap), alpha, x, f, gradient); /* YES! hrue */
return status;
}
update_position(&(state->wrap), alpha, x, f, gradient);
state->delta_f = *f - f0;
/*
* Choose a new direction for the next step
*/
{
/*
* This is the BFGS update:
*/
/*
* p' = g1 - A dx - B dg
*/
/*
* A = - (1+ dg.dg/dx.dg) B + dg.g/dx.dg
*/
/*
* B = dx.g/dx.dg
*/
gsl_vector *dx0 = state->dx0;
gsl_vector *dg0 = state->dg0;
double dxg, dgg, dxdg, dgnorm, A, B;
/*
* dx0 = x - x0
*/
gsl_vector_memcpy(dx0, x);
gsl_blas_daxpy(-1.0, x0, dx0);
gsl_vector_memcpy(dx, dx0); /* keep a copy */
/*
* dg0 = g - g0
*/
gsl_vector_memcpy(dg0, gradient);
gsl_blas_daxpy(-1.0, g0, dg0);
gsl_blas_ddot(dx0, gradient, &dxg);
gsl_blas_ddot(dg0, gradient, &dgg);
gsl_blas_ddot(dx0, dg0, &dxdg);
dgnorm = gsl_blas_dnrm2(dg0);
if (dxdg != 0) {
B = dxg / dxdg;
A = -(1.0 + dgnorm * dgnorm / dxdg) * B + dgg / dxdg;
} else {
B = 0;
A = 0;
}
gsl_vector_memcpy(p, gradient);
gsl_blas_daxpy(-A, dx0, p);
gsl_blas_daxpy(-B, dg0, p);
}
gsl_vector_memcpy(g0, gradient);
gsl_vector_memcpy(x0, x);
state->g0norm = gsl_blas_dnrm2(g0);
state->pnorm = gsl_blas_dnrm2(p);
/*
* update direction and fp0
*/
gsl_blas_ddot(p, gradient, &pg);
dir = (pg >= 0.0) ? -1.0 : +1.0;
gsl_blas_dscal(dir / state->pnorm, p);
state->pnorm = gsl_blas_dnrm2(p);
gsl_blas_ddot(p, g0, &state->fp0);
change_direction(&state->wrap);
return GSL_SUCCESS;
}
void forward_affine(gsl_matrix* weightMat, gsl_vector* bias_vector, gsl_vector* input_vector, gsl_vector* output_vector){
// inner product weights*input
gsl_blas_dgemv(CblasNoTrans, 1, weightMat, input_vector, 0, output_vector);
// add bias
gsl_blas_daxpy(1, bias_vector, output_vector);
}
//Train all the samples using naive stochastic gradient descent
tnn_error tnn_trainer_class_train_nsgd(tnn_trainer_class *t, gsl_matrix *inputs, size_t *labels){
tnn_error ret;
tnn_state *sin;
tnn_param *p;
gsl_vector *rd;
gsl_vector *pw;
gsl_vector_view in;
gsl_vector_view lb;
double eps;
size_t i,j;
//Routine check
if(t->t != TNN_TRAINER_CLASS_TYPE_NSGD){
return TNN_ERROR_TRAINER_CLASS_MISTYPE;
}
//Check the input
TNN_MACRO_ERRORTEST(tnn_machine_get_sin(&t->m, &sin),ret);
if(inputs->size2 != sin->size){
return TNN_ERROR_STATE_INCOMP;
}
//Set the loss output dx to be 1
gsl_vector_set(&t->l.output->dx, 0, 1.0);
//Get the parameter and allocate rd and pw
TNN_MACRO_ERRORTEST(tnn_machine_get_param(&t->m, &p), ret);
rd = gsl_vector_alloc(p->size);
pw = gsl_vector_alloc(p->size);
if(rd == NULL || pw == NULL){
return TNN_ERROR_GSL;
}
//Into the main loop
for(eps = DBL_MAX, ((tnn_trainer_class_nsgd*)t->c)->titer = 0;
eps > ((tnn_trainer_class_nsgd*)t->c)->epsilon && ((tnn_trainer_class_nsgd*)t->c)->titer < ((tnn_trainer_class_nsgd*)t->c)->niter;
((tnn_trainer_class_nsgd*)t->c)->titer = ((tnn_trainer_class_nsgd*)t->c)->titer + ((tnn_trainer_class_nsgd*)t->c)->eiter){
//Copy the previous pw
TNN_MACRO_GSLTEST(gsl_blas_dcopy(p->x, pw));
for(i = 0; i < ((tnn_trainer_class_nsgd*)t->c)->eiter; i = i + 1){
j = (((tnn_trainer_class_nsgd*)t->c)->titer + i)%inputs->size1;
//Check the label
if(labels[j] >= t->lset->size1){
return TNN_ERROR_STATE_INCOMP;
}
//Get the inputs and label vector
lb = gsl_matrix_row(t->lset, labels[j]);
in = gsl_matrix_row(inputs, j);
//Copy the data into the input/label and do forward and backward propagation
TNN_MACRO_GSLTEST(gsl_blas_dcopy(&in.vector, &sin->x));
TNN_MACRO_GSLTEST(gsl_blas_dcopy(&lb.vector, &t->label->x));
TNN_MACRO_ERRORTEST(tnn_machine_fprop(&t->m), ret);
TNN_MACRO_ERRORTEST(tnn_loss_fprop(&t->l), ret);
TNN_MACRO_ERRORTEST(tnn_loss_bprop(&t->l), ret);
TNN_MACRO_ERRORTEST(tnn_machine_bprop(&t->m), ret);
//Compute the accumulated regularization paramter
TNN_MACRO_ERRORTEST(tnn_reg_d(&t->r, p->x, rd), ret);
TNN_MACRO_GSLTEST(gsl_blas_daxpy(t->lambda, rd, p->dx));
//Compute the parameter update
TNN_MACRO_GSLTEST(gsl_blas_daxpy(-((tnn_trainer_class_nsgd*)t->c)->eta, p->dx, p->x));
}
//Compute the 2 square norm of difference of p as eps
TNN_MACRO_GSLTEST(gsl_blas_daxpy(-1.0, p->x, pw));
eps = gsl_blas_dnrm2(pw);
}
return TNN_ERROR_SUCCESS;
}
static void
nonsymmv_get_right_eigenvectors(gsl_matrix *T, gsl_matrix *Z,
gsl_vector_complex *eval,
gsl_matrix_complex *evec,
gsl_eigen_nonsymmv_workspace *w)
{
const size_t N = T->size1;
const double smlnum = GSL_DBL_MIN * N / GSL_DBL_EPSILON;
const double bignum = (1.0 - GSL_DBL_EPSILON) / smlnum;
int i; /* looping */
size_t iu, /* looping */
ju,
ii;
gsl_complex lambda; /* current eigenvalue */
double lambda_re, /* Re(lambda) */
lambda_im; /* Im(lambda) */
gsl_matrix_view Tv, /* temporary views */
Zv;
gsl_vector_view y, /* temporary views */
y2,
ev,
ev2;
double dat[4], /* scratch arrays */
dat_X[4];
double scale; /* scale factor */
double xnorm; /* |X| */
gsl_vector_complex_view ecol, /* column of evec */
ecol2;
int complex_pair; /* complex eigenvalue pair? */
double smin;
/*
* Compute 1-norm of each column of upper triangular part of T
* to control overflow in triangular solver
*/
gsl_vector_set(w->work3, 0, 0.0);
for (ju = 1; ju < N; ++ju)
{
gsl_vector_set(w->work3, ju, 0.0);
for (iu = 0; iu < ju; ++iu)
{
gsl_vector_set(w->work3, ju,
gsl_vector_get(w->work3, ju) +
fabs(gsl_matrix_get(T, iu, ju)));
}
}
for (i = (int) N - 1; i >= 0; --i)
{
iu = (size_t) i;
/* get current eigenvalue and store it in lambda */
lambda_re = gsl_matrix_get(T, iu, iu);
if (iu != 0 && gsl_matrix_get(T, iu, iu - 1) != 0.0)
{
lambda_im = sqrt(fabs(gsl_matrix_get(T, iu, iu - 1))) *
sqrt(fabs(gsl_matrix_get(T, iu - 1, iu)));
}
else
{
lambda_im = 0.0;
}
GSL_SET_COMPLEX(&lambda, lambda_re, lambda_im);
smin = GSL_MAX(GSL_DBL_EPSILON * (fabs(lambda_re) + fabs(lambda_im)),
smlnum);
smin = GSL_MAX(smin, GSL_NONSYMMV_SMLNUM);
if (lambda_im == 0.0)
{
int k, l;
gsl_vector_view bv, xv;
/* real eigenvector */
/*
* The ordering of eigenvalues in 'eval' is arbitrary and
* does not necessarily follow the Schur form T, so store
* lambda in the right slot in eval to ensure it corresponds
* to the eigenvector we are about to compute
*/
gsl_vector_complex_set(eval, iu, lambda);
/*
* We need to solve the system:
*
* (T(1:iu-1, 1:iu-1) - lambda*I)*X = -T(1:iu-1,iu)
*/
/* construct right hand side */
for (k = 0; k < i; ++k)
{
gsl_vector_set(w->work,
(size_t) k,
-gsl_matrix_get(T, (size_t) k, iu));
}
gsl_vector_set(w->work, iu, 1.0);
for (l = i - 1; l >= 0; --l)
{
size_t lu = (size_t) l;
if (lu == 0)
complex_pair = 0;
else
complex_pair = gsl_matrix_get(T, lu, lu - 1) != 0.0;
if (!complex_pair)
{
double x;
/*
* 1-by-1 diagonal block - solve the system:
*
* (T_{ll} - lambda)*x = -T_{l(iu)}
*/
Tv = gsl_matrix_submatrix(T, lu, lu, 1, 1);
bv = gsl_vector_view_array(dat, 1);
gsl_vector_set(&bv.vector, 0,
gsl_vector_get(w->work, lu));
xv = gsl_vector_view_array(dat_X, 1);
gsl_schur_solve_equation(1.0,
&Tv.matrix,
lambda_re,
1.0,
1.0,
&bv.vector,
&xv.vector,
&scale,
&xnorm,
smin);
/* scale x to avoid overflow */
x = gsl_vector_get(&xv.vector, 0);
if (xnorm > 1.0)
{
if (gsl_vector_get(w->work3, lu) > bignum / xnorm)
{
x /= xnorm;
scale /= xnorm;
}
}
if (scale != 1.0)
{
gsl_vector_view wv;
wv = gsl_vector_subvector(w->work, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
}
gsl_vector_set(w->work, lu, x);
if (lu > 0)
{
gsl_vector_view v1, v2;
/* update right hand side */
v1 = gsl_matrix_subcolumn(T, lu, 0, lu);
v2 = gsl_vector_subvector(w->work, 0, lu);
gsl_blas_daxpy(-x, &v1.vector, &v2.vector);
} /* if (l > 0) */
} /* if (!complex_pair) */
else
{
double x11, x21;
/*
* 2-by-2 diagonal block
*/
Tv = gsl_matrix_submatrix(T, lu - 1, lu - 1, 2, 2);
bv = gsl_vector_view_array(dat, 2);
gsl_vector_set(&bv.vector, 0,
gsl_vector_get(w->work, lu - 1));
gsl_vector_set(&bv.vector, 1,
gsl_vector_get(w->work, lu));
xv = gsl_vector_view_array(dat_X, 2);
gsl_schur_solve_equation(1.0,
&Tv.matrix,
lambda_re,
1.0,
1.0,
&bv.vector,
&xv.vector,
&scale,
&xnorm,
smin);
/* scale X(1,1) and X(2,1) to avoid overflow */
x11 = gsl_vector_get(&xv.vector, 0);
x21 = gsl_vector_get(&xv.vector, 1);
if (xnorm > 1.0)
{
double beta;
beta = GSL_MAX(gsl_vector_get(w->work3, lu - 1),
gsl_vector_get(w->work3, lu));
if (beta > bignum / xnorm)
{
x11 /= xnorm;
x21 /= xnorm;
scale /= xnorm;
}
}
/* scale if necessary */
if (scale != 1.0)
{
gsl_vector_view wv;
wv = gsl_vector_subvector(w->work, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
}
gsl_vector_set(w->work, lu - 1, x11);
gsl_vector_set(w->work, lu, x21);
/* update right hand side */
if (lu > 1)
{
gsl_vector_view v1, v2;
v1 = gsl_matrix_subcolumn(T, lu - 1, 0, lu - 1);
v2 = gsl_vector_subvector(w->work, 0, lu - 1);
gsl_blas_daxpy(-x11, &v1.vector, &v2.vector);
v1 = gsl_matrix_subcolumn(T, lu, 0, lu - 1);
gsl_blas_daxpy(-x21, &v1.vector, &v2.vector);
}
--l;
} /* if (complex_pair) */
} /* for (l = i - 1; l >= 0; --l) */
/*
* At this point, w->work is an eigenvector of the
* Schur form T. To get an eigenvector of the original
* matrix, we multiply on the left by Z, the matrix of
* Schur vectors
*/
ecol = gsl_matrix_complex_column(evec, iu);
y = gsl_matrix_column(Z, iu);
if (iu > 0)
{
gsl_vector_view x;
Zv = gsl_matrix_submatrix(Z, 0, 0, N, iu);
x = gsl_vector_subvector(w->work, 0, iu);
/* compute Z * w->work and store it in Z(:,iu) */
gsl_blas_dgemv(CblasNoTrans,
1.0,
&Zv.matrix,
&x.vector,
gsl_vector_get(w->work, iu),
&y.vector);
} /* if (iu > 0) */
/* store eigenvector into evec */
ev = gsl_vector_complex_real(&ecol.vector);
ev2 = gsl_vector_complex_imag(&ecol.vector);
scale = 0.0;
for (ii = 0; ii < N; ++ii)
{
double a = gsl_vector_get(&y.vector, ii);
/* store real part of eigenvector */
gsl_vector_set(&ev.vector, ii, a);
/* set imaginary part to 0 */
gsl_vector_set(&ev2.vector, ii, 0.0);
if (fabs(a) > scale)
scale = fabs(a);
}
if (scale != 0.0)
scale = 1.0 / scale;
/* scale by magnitude of largest element */
gsl_blas_dscal(scale, &ev.vector);
} /* if (GSL_IMAG(lambda) == 0.0) */
else
{
gsl_vector_complex_view bv, xv;
size_t k;
int l;
gsl_complex lambda2;
/* complex eigenvector */
/*
* Store the complex conjugate eigenvalues in the right
* slots in eval
*/
GSL_SET_REAL(&lambda2, GSL_REAL(lambda));
GSL_SET_IMAG(&lambda2, -GSL_IMAG(lambda));
gsl_vector_complex_set(eval, iu - 1, lambda);
gsl_vector_complex_set(eval, iu, lambda2);
/*
* First solve:
*
* [ T(i:i+1,i:i+1) - lambda*I ] * X = 0
*/
if (fabs(gsl_matrix_get(T, iu - 1, iu)) >=
fabs(gsl_matrix_get(T, iu, iu - 1)))
{
gsl_vector_set(w->work, iu - 1, 1.0);
gsl_vector_set(w->work2, iu,
lambda_im / gsl_matrix_get(T, iu - 1, iu));
}
else
{
gsl_vector_set(w->work, iu - 1,
-lambda_im / gsl_matrix_get(T, iu, iu - 1));
gsl_vector_set(w->work2, iu, 1.0);
}
gsl_vector_set(w->work, iu, 0.0);
gsl_vector_set(w->work2, iu - 1, 0.0);
/* construct right hand side */
for (k = 0; k < iu - 1; ++k)
{
gsl_vector_set(w->work, k,
-gsl_vector_get(w->work, iu - 1) *
gsl_matrix_get(T, k, iu - 1));
gsl_vector_set(w->work2, k,
-gsl_vector_get(w->work2, iu) *
gsl_matrix_get(T, k, iu));
}
/*
* We must solve the upper quasi-triangular system:
*
* [ T(1:i-2,1:i-2) - lambda*I ] * X = s*(work + i*work2)
*/
for (l = i - 2; l >= 0; --l)
{
size_t lu = (size_t) l;
if (lu == 0)
complex_pair = 0;
else
complex_pair = gsl_matrix_get(T, lu, lu - 1) != 0.0;
if (!complex_pair)
{
gsl_complex bval;
gsl_complex x;
/*
* 1-by-1 diagonal block - solve the system:
*
* (T_{ll} - lambda)*x = work + i*work2
*/
Tv = gsl_matrix_submatrix(T, lu, lu, 1, 1);
bv = gsl_vector_complex_view_array(dat, 1);
xv = gsl_vector_complex_view_array(dat_X, 1);
GSL_SET_COMPLEX(&bval,
gsl_vector_get(w->work, lu),
gsl_vector_get(w->work2, lu));
gsl_vector_complex_set(&bv.vector, 0, bval);
gsl_schur_solve_equation_z(1.0,
&Tv.matrix,
&lambda,
1.0,
1.0,
&bv.vector,
&xv.vector,
&scale,
&xnorm,
smin);
if (xnorm > 1.0)
{
if (gsl_vector_get(w->work3, lu) > bignum / xnorm)
{
gsl_blas_zdscal(1.0/xnorm, &xv.vector);
scale /= xnorm;
}
}
/* scale if necessary */
if (scale != 1.0)
{
gsl_vector_view wv;
wv = gsl_vector_subvector(w->work, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
wv = gsl_vector_subvector(w->work2, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
}
x = gsl_vector_complex_get(&xv.vector, 0);
gsl_vector_set(w->work, lu, GSL_REAL(x));
gsl_vector_set(w->work2, lu, GSL_IMAG(x));
/* update the right hand side */
if (lu > 0)
{
gsl_vector_view v1, v2;
v1 = gsl_matrix_subcolumn(T, lu, 0, lu);
v2 = gsl_vector_subvector(w->work, 0, lu);
gsl_blas_daxpy(-GSL_REAL(x), &v1.vector, &v2.vector);
v2 = gsl_vector_subvector(w->work2, 0, lu);
gsl_blas_daxpy(-GSL_IMAG(x), &v1.vector, &v2.vector);
} /* if (lu > 0) */
} /* if (!complex_pair) */
else
{
gsl_complex b1, b2, x1, x2;
/*
* 2-by-2 diagonal block - solve the system
*/
Tv = gsl_matrix_submatrix(T, lu - 1, lu - 1, 2, 2);
bv = gsl_vector_complex_view_array(dat, 2);
xv = gsl_vector_complex_view_array(dat_X, 2);
GSL_SET_COMPLEX(&b1,
gsl_vector_get(w->work, lu - 1),
gsl_vector_get(w->work2, lu - 1));
GSL_SET_COMPLEX(&b2,
gsl_vector_get(w->work, lu),
gsl_vector_get(w->work2, lu));
gsl_vector_complex_set(&bv.vector, 0, b1);
gsl_vector_complex_set(&bv.vector, 1, b2);
gsl_schur_solve_equation_z(1.0,
&Tv.matrix,
&lambda,
1.0,
1.0,
&bv.vector,
&xv.vector,
&scale,
&xnorm,
smin);
x1 = gsl_vector_complex_get(&xv.vector, 0);
x2 = gsl_vector_complex_get(&xv.vector, 1);
if (xnorm > 1.0)
{
double beta;
beta = GSL_MAX(gsl_vector_get(w->work3, lu - 1),
gsl_vector_get(w->work3, lu));
if (beta > bignum / xnorm)
{
gsl_blas_zdscal(1.0/xnorm, &xv.vector);
scale /= xnorm;
}
}
/* scale if necessary */
if (scale != 1.0)
{
gsl_vector_view wv;
wv = gsl_vector_subvector(w->work, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
wv = gsl_vector_subvector(w->work2, 0, iu + 1);
gsl_blas_dscal(scale, &wv.vector);
}
gsl_vector_set(w->work, lu - 1, GSL_REAL(x1));
gsl_vector_set(w->work, lu, GSL_REAL(x2));
gsl_vector_set(w->work2, lu - 1, GSL_IMAG(x1));
gsl_vector_set(w->work2, lu, GSL_IMAG(x2));
/* update right hand side */
if (lu > 1)
{
gsl_vector_view v1, v2, v3, v4;
v1 = gsl_matrix_subcolumn(T, lu - 1, 0, lu - 1);
v4 = gsl_matrix_subcolumn(T, lu, 0, lu - 1);
v2 = gsl_vector_subvector(w->work, 0, lu - 1);
v3 = gsl_vector_subvector(w->work2, 0, lu - 1);
gsl_blas_daxpy(-GSL_REAL(x1), &v1.vector, &v2.vector);
gsl_blas_daxpy(-GSL_REAL(x2), &v4.vector, &v2.vector);
gsl_blas_daxpy(-GSL_IMAG(x1), &v1.vector, &v3.vector);
gsl_blas_daxpy(-GSL_IMAG(x2), &v4.vector, &v3.vector);
} /* if (lu > 1) */
--l;
} /* if (complex_pair) */
} /* for (l = i - 2; l >= 0; --l) */
/*
* At this point, work + i*work2 is an eigenvector
* of T - backtransform to get an eigenvector of the
* original matrix
*/
y = gsl_matrix_column(Z, iu - 1);
y2 = gsl_matrix_column(Z, iu);
if (iu > 1)
{
gsl_vector_view x;
/* compute real part of eigenvectors */
Zv = gsl_matrix_submatrix(Z, 0, 0, N, iu - 1);
x = gsl_vector_subvector(w->work, 0, iu - 1);
gsl_blas_dgemv(CblasNoTrans,
1.0,
&Zv.matrix,
&x.vector,
gsl_vector_get(w->work, iu - 1),
&y.vector);
/* now compute the imaginary part */
x = gsl_vector_subvector(w->work2, 0, iu - 1);
gsl_blas_dgemv(CblasNoTrans,
1.0,
&Zv.matrix,
&x.vector,
gsl_vector_get(w->work2, iu),
&y2.vector);
}
else
{
gsl_blas_dscal(gsl_vector_get(w->work, iu - 1), &y.vector);
gsl_blas_dscal(gsl_vector_get(w->work2, iu), &y2.vector);
}
/*
* Now store the eigenvectors into evec - the real parts
* are Z(:,iu - 1) and the imaginary parts are
* +/- Z(:,iu)
*/
/* get views of the two eigenvector slots */
ecol = gsl_matrix_complex_column(evec, iu - 1);
ecol2 = gsl_matrix_complex_column(evec, iu);
/*
* save imaginary part first as it may get overwritten
* when copying the real part due to our storage scheme
* in Z/evec
*/
ev = gsl_vector_complex_imag(&ecol.vector);
ev2 = gsl_vector_complex_imag(&ecol2.vector);
scale = 0.0;
for (ii = 0; ii < N; ++ii)
{
double a = gsl_vector_get(&y2.vector, ii);
scale = GSL_MAX(scale,
fabs(a) + fabs(gsl_vector_get(&y.vector, ii)));
gsl_vector_set(&ev.vector, ii, a);
gsl_vector_set(&ev2.vector, ii, -a);
}
/* now save the real part */
ev = gsl_vector_complex_real(&ecol.vector);
ev2 = gsl_vector_complex_real(&ecol2.vector);
for (ii = 0; ii < N; ++ii)
{
double a = gsl_vector_get(&y.vector, ii);
gsl_vector_set(&ev.vector, ii, a);
gsl_vector_set(&ev2.vector, ii, a);
}
if (scale != 0.0)
scale = 1.0 / scale;
/* scale by largest element magnitude */
gsl_blas_zdscal(scale, &ecol.vector);
gsl_blas_zdscal(scale, &ecol2.vector);
/*
* decrement i since we took care of two eigenvalues at
* the same time
*/
--i;
} /* if (GSL_IMAG(lambda) != 0.0) */
} /* for (i = (int) N - 1; i >= 0; --i) */
} /* nonsymmv_get_right_eigenvectors() */
static int
conjugate_fr_iterate (void *vstate, gsl_multimin_function_fdf * fdf,
gsl_vector * x, double *f,
gsl_vector * gradient, gsl_vector * dx)
{
conjugate_fr_state_t *state = (conjugate_fr_state_t *) vstate;
gsl_vector *x1 = state->x1;
gsl_vector *dx1 = state->dx1;
gsl_vector *x2 = state->x2;
gsl_vector *p = state->p;
gsl_vector *g0 = state->g0;
double pnorm = state->pnorm;
double g0norm = state->g0norm;
double fa = *f, fb, fc;
double dir;
double stepa = 0.0, stepb, stepc = state->step, tol = state->tol;
double g1norm;
double pg;
if (pnorm == 0.0 || g0norm == 0.0)
{
gsl_vector_set_zero (dx);
return GSL_ENOPROG;
}
/* Determine which direction is downhill, +p or -p */
gsl_blas_ddot (p, gradient, &pg);
dir = (pg >= 0.0) ? +1.0 : -1.0;
/* Compute new trial point at x_c= x - step * p, where p is the
current direction */
take_step (x, p, stepc, dir / pnorm, x1, dx);
/* Evaluate function and gradient at new point xc */
fc = GSL_MULTIMIN_FN_EVAL_F (fdf, x1);
if (fc < fa)
{
/* Success, reduced the function value */
state->step = stepc * 2.0;
*f = fc;
gsl_vector_memcpy (x, x1);
GSL_MULTIMIN_FN_EVAL_DF (fdf, x1, gradient);
return GSL_SUCCESS;
}
#ifdef DEBUG
printf ("got stepc = %g fc = %g\n", stepc, fc);
#endif
/* Do a line minimisation in the region (xa,fa) (xc,fc) to find an
intermediate (xb,fb) satisifying fa > fb < fc. Choose an initial
xb based on parabolic interpolation */
intermediate_point (fdf, x, p, dir / pnorm, pg,
stepa, stepc, fa, fc, x1, dx1, gradient, &stepb, &fb);
if (stepb == 0.0)
{
return GSL_ENOPROG;
}
minimize (fdf, x, p, dir / pnorm,
stepa, stepb, stepc, fa, fb, fc, tol,
x1, dx1, x2, dx, gradient, &(state->step), f, &g1norm);
gsl_vector_memcpy (x, x2);
/* Choose a new conjugate direction for the next step */
state->iter = (state->iter + 1) % x->size;
if (state->iter == 0)
{
gsl_vector_memcpy (p, gradient);
state->pnorm = g1norm;
}
else
{
/* p' = g1 - beta * p */
double beta = -pow (g1norm / g0norm, 2.0);
gsl_blas_dscal (-beta, p);
gsl_blas_daxpy (1.0, gradient, p);
state->pnorm = gsl_blas_dnrm2 (p);
}
state->g0norm = g1norm;
gsl_vector_memcpy (g0, gradient);
#ifdef DEBUG
printf ("updated conjugate directions\n");
printf ("p: ");
gsl_vector_fprintf (stdout, p, "%g");
printf ("g: ");
gsl_vector_fprintf (stdout, gradient, "%g");
#endif
return GSL_SUCCESS;
}
void Vector::axpy ( const double alpha, const Vector& that ) {
gsl_blas_daxpy( alpha, &that.vector, &vector );
}
static int
bundle_method_iterate (void *vstate, gsl_multimin_function_fsdf * fsdf, gsl_vector * x, double * f,
gsl_vector * subgradient, gsl_vector * dx, double * eps)
{
bundle_method_state_t *state = (bundle_method_state_t *) vstate;
bundle_element *item;
size_t i, debug=0;
int status;
double tmp_d, t_old, t_int_l; /* local variables */
gsl_vector *y; /* a trial point (the next iteration point by the serios step) */
gsl_vector *sgr_y; /* subgradient at y */
double f_y; /* the function value at y */
gsl_vector *p; /* the aggregate subgradient */
double p_norm, lin_error_p; /* norm of p, the aggregate linear. error */
gsl_vector *tmp_v;
/* data for the convex quadratic problem (for the dual problem) */
gsl_vector *q; /* elements of the array are the linearization errors */
gsl_matrix *Q; /* Q=G^T*G (G is matrix which collumns are subgradients) */
gsl_vector *lambda; /* the convex combination coefficients of the subgradients (solution of the dual problem) */
lambda = gsl_vector_alloc(state->bundle_size);
if(lambda == 0)
{
GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0);
}
q = gsl_vector_alloc(lambda->size);
if(q == 0)
{
gsl_vector_free(lambda);
GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0);
}
y = gsl_vector_calloc(x->size);
if(y == 0)
{
gsl_vector_free(q);
gsl_vector_free(lambda);
GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0);
}
sgr_y = gsl_vector_calloc(x->size);
if(sgr_y == 0)
{
gsl_vector_free(y);
gsl_vector_free(q);
gsl_vector_free(lambda);
GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0);
}
Q = gsl_matrix_alloc(state->bundle_size, state->bundle_size);
if(Q == 0)
{
gsl_vector_free(sgr_y);
gsl_vector_free(y);
gsl_vector_free(q);
gsl_vector_free(lambda);
GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0);
}
p = gsl_vector_calloc(x->size);
if(p == 0)
{
gsl_matrix_free(Q);
gsl_vector_free(sgr_y);
gsl_vector_free(y);
gsl_vector_free(q);
gsl_vector_free(lambda);
GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0);
}
tmp_v = gsl_vector_calloc(x->size);
if(tmp_v == 0)
{
gsl_vector_free(p);
gsl_matrix_free(Q);
gsl_vector_free(sgr_y);
gsl_vector_free(y);
gsl_vector_free(q);
gsl_vector_free(lambda);
GSL_ERROR_VAL ("failed to allocate workspace", GSL_ENOMEM, 0);
}
/* solve the dual problem */
status = build_cqp_data(state, Q, q);
status = solve_qp_pdip(Q, q, lambda);
gsl_matrix_free(Q);
gsl_vector_free(q);
/* compute the aggregate subgradient (it is called p in the documantation)*/
/* and the appropriated linearization error */
lin_error_p = 0.0;
item = state->head;
for(i=0; i<lambda->size; i++)
{
status = gsl_blas_daxpy(gsl_vector_get(lambda,i), item->sgr, p);
lin_error_p += gsl_vector_get(lambda,i)*(item->lin_error);
item = item->next;
}
if(debug)
{
printf("the dual problem solution:\n");
for(i=0;i<lambda->size;i++)
printf("%7.6e ",gsl_vector_get(lambda,i));
printf("\n\n");
printf("the aggregate subgradient: \n");
for(i=0;i<p->size;i++)
printf("%.6e ",gsl_vector_get(p,i));
printf("\n");
printf("lin. error for aggr subgradient = %e\n",lin_error_p);
}
/* the norm of the aggr subgradient */
p_norm = gsl_blas_dnrm2(p);
/* search direction dx=-t*p (t is the length of step) */
status = gsl_vector_memcpy(dx,p);
status = gsl_vector_scale(dx,-1.0*state->t);
/* v =-t*norm(p)^2-alpha_p */
state->v = -gsl_pow_2(p_norm)*(state->t)-lin_error_p;
/* the subgradient is the aggegate sungradient */
status = gsl_blas_dcopy(p,subgradient);
/* iteration step */
/* y=x+dx */
status = gsl_blas_dcopy(dx,y);
status = gsl_blas_daxpy(1.0,x,y);
/* function value at y */
f_y = GSL_MULTIMIN_FN_EVAL_F(fsdf, y);
state->f_eval++;
/* for t-update */
if(!state->fixed_step_length)
{
t_old = state->t;
if(fabs(state->v-(f_y-*f)) < state->rg || state->v-(f_y-*f) > state->rg)
t_int_l = state->t_max;
else
t_int_l = 0.5*t_old*(state->v)/(state->v-(f_y-*f));
}
else
{
t_old = state->t;
t_int_l = state->t;
}
if( f_y-*f <= state->m_ss*state->v ) /* Serious-Step */
{
if(debug)
printf("\nSerious-Step\n");
/* the relaxation step */
if(state->relaxation)
{
if(f_y-*f <= state->v*state->m_rel)
{
double f_z;
gsl_vector * z = gsl_vector_alloc(y->size);
/* z = y+dx = x+2*dx */
status = gsl_blas_dcopy(x,z);
status = gsl_blas_daxpy(2.0,dx,z);
f_z = GSL_MULTIMIN_FN_EVAL_F(fsdf, z);
state->f_eval++;
if(0.5*f_z-f_y+0.5*(*f) > state->rg)
state->rel_parameter = GSL_MIN_DBL(-0.5*(-0.5*f_z+2.0*f_y-1.5*(*f))/(0.5*f_z-f_y+0.5*(*f)),1.999);
else if (fabs(0.5*f_z-f_y+0.5*(*f)) > state->rg)
state->rel_parameter = 1.999;
else
/* something is wrong */
state->rel_parameter = 1.0;
/* save the old iteration point */
status = gsl_blas_dcopy(y,z);
/* y = (1-rel_parameter)*x+rel_parameter*y */
gsl_blas_dscal(state->rel_parameter,y);
status = gsl_blas_daxpy(1.0-state->rel_parameter,x,y);
/* f(y) und sgr_f(y) */
tmp_d = GSL_MULTIMIN_FN_EVAL_F(fsdf, y);
state->f_eval++;
if(tmp_d > f_y)
{
/* keep y as the current point */
status = gsl_blas_dcopy(z,y);
state->rel_counter++;
}
else
{
f_y = tmp_d;
/* dx = y-x */
status = gsl_blas_dcopy(y,dx);
status = gsl_blas_daxpy(-1.0,x,dx);
/* if iteration points bevor and after the rel. step are closly,
the rel_step counte will be increased */
/* |1-rel_parameter| <= 0.1*/
if( fabs(1.0-state->rel_parameter) < 0.1)
state->rel_counter++;
}
GSL_MULTIMIN_FN_EVAL_SDF(fsdf, y, sgr_y);
state->sgr_eval++;
if(state->rel_counter > state->rel_counter_max)
state->relaxation = 0;
/* */
status = gsl_blas_daxpy(-1.0,y,z);
status = gsl_blas_ddot(p, z, &tmp_d);
*eps = f_y-*f-(state->v)+tmp_d;
gsl_vector_free(z);
}
else
{
*eps = f_y-(state->v)-*f;
GSL_MULTIMIN_FN_EVAL_SDF(fsdf, y, sgr_y);
state->sgr_eval++;
}
}
else
{
*eps = f_y-(state->v)-*f;
GSL_MULTIMIN_FN_EVAL_SDF(fsdf, y, sgr_y);
state->sgr_eval++;
}
/* calculate linearization errors at new iteration point */
item = state->head;
for(i=0; i<state->bundle_size; i++)
{
status = gsl_blas_ddot(item->sgr, dx, &tmp_d);
item->lin_error += f_y-*f-tmp_d;
item = item->next;
}
/* linearization error at new iteration point */
status = gsl_blas_ddot(p, dx, &tmp_d);
lin_error_p += f_y-*f-tmp_d;
/* update the bundle */
status = update_bundle(state, sgr_y, 0.0, lambda, p, lin_error_p, 1);
/* adapt the step length */
if(!state->fixed_step_length)
{
if(f_y-*f <= state->v*state->m_t && state->step_counter > 0)
state->t = t_int_l;
else if(state->step_counter>3)
state->t=2.0*t_old;
state->t = GSL_MIN_DBL(GSL_MIN_DBL(state->t,10.0*t_old),state->t_max);
/*state->eps_v = GSL_MAX_DBL(state->eps_v,-2.0*state->v);*/
state->step_counter = GSL_MAX_INT(state->step_counter+1,1);
if(fabs(state->t-t_old) > state->rg)
state->step_counter=1;
}
/* x=y, f=f(y) */
status = gsl_blas_dcopy(y,x);
*f = f_y;
}
else /* Null-Step */
{
if(debug)
printf("\nNull-Step\n");
GSL_MULTIMIN_FN_EVAL_SDF(fsdf, y, sgr_y);
state->sgr_eval++;
/* eps for the eps_subdifferential */
*eps = lin_error_p;
/*calculate the liniarization error at y */
status = gsl_blas_ddot(sgr_y,dx,&tmp_d);
tmp_d += *f-f_y;
/* Bundle update */
status = update_bundle(state, sgr_y, tmp_d, lambda, p, lin_error_p, 0);
/* adapt the step length */
if(!state->fixed_step_length)
{
/*state->eps_v = GSL_MIN_DBL(state->eps_v,lin_error_p);*/
if(tmp_d > GSL_MAX_DBL(p_norm,lin_error_p) && state->step_counter < -1)
state->t = t_int_l;
else if(state->step_counter < -3)
state->t = 0.5*t_old;
state->t = GSL_MAX_DBL(GSL_MAX_DBL(0.1*t_old,state->t),state->t_min);
state->step_counter = GSL_MIN_INT(state->step_counter-1,-1);
if(fabs(state->t-t_old) > state->rg)
state->step_counter = -1;
}
}
state->lambda_min = p_norm * state->lm_accuracy;
if(debug)
{
printf("\nthe new bundle:\n");
bundle_out_liste(state);
printf("\n\n");
printf("the curent itarationspoint (1 x %d)\n",x->size);
for(i=0;i<x->size;i++)
printf("%12.6f ",gsl_vector_get(x,i));
printf("\n\n");
printf("functions value at current point: f=%.8f\n",*f);
printf("\nstep length t=%.5e\n",state->t);
printf("\nstep_counter sc=%d\n",state->step_counter);
printf("\naccuracy: v=%.5e\n",state->v);
printf("\nlambda_min=%e\n",state->lambda_min);
printf("\n");
}
gsl_vector_free(lambda);
gsl_vector_free(y);
gsl_vector_free(sgr_y);
gsl_vector_free(p);
return GSL_SUCCESS;
}
/**
* C++ version of gsl_blas_daxpy().
* @param alpha A constant
* @param X A vector
* @param Y A vector
* @return Error code on failure
*/
int daxpy( double alpha, vector const& X, vector& Y ){
return gsl_blas_daxpy( alpha, X.get(), Y.get() ); }
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);
}
