コード例 #1
0
ファイル: KLInference.cpp プロジェクト: minxuancao/shogun
SGVector<float64_t> CKLInference::get_derivative_wrt_kernel(const TParameter* param)
{
	REQUIRE(param, "Param not set\n");
	SGVector<float64_t> result;
	int64_t len=const_cast<TParameter *>(param)->m_datatype.get_num_elements();
	result=SGVector<float64_t>(len);

	for (index_t i=0; i<result.vlen; i++)
	{
		SGMatrix<float64_t> dK(m_mu.vlen, m_mu.vlen);

		//dK = feval(covfunc{:},hyper,x,j);
		if (result.vlen==1)
			dK=m_kernel->get_parameter_gradient(param);
		else
			dK=m_kernel->get_parameter_gradient(param, i);

		result[i]=get_derivative_related_cov(dK);
		result[i]*=CMath::exp(m_log_scale*2.0);
	}

	return result;
}
コード例 #2
0
ファイル: KSDS-deriv.cpp プロジェクト: Bazman76/KFKSDS
void KFKSDS_deriv_C (int *dim, double *sy, double *sZ, double *sT, double *sH, 
  double *sR, double *sV, double *sQ, double *sa0, double *sP0, double *dvof, 
  double *epshat, double *vareps, double *etahat, double *vareta, 
  double *r, double *N, double *dr, double *dN, 
  double *dahat, double *dvareps)
{
  //int s, p = dim[1], mp1 = m + 1;
  int i, ip1, j, k, n = dim[0], m = dim[2], 
    ir = dim[3], rp1 = ir + 1, nrp1 = n * rp1,
    rp1m = rp1 * m, iaux, irp1m,
    irsod = ir * sizeof(double), msod = m * sizeof(double), 
    nsod = n * sizeof(double), rp1msod = rp1 * msod;

  //double invf[n], vof[n], msHsq, dfinvfsq[nrp1];
  double msHsq;
  std::vector<double> invf(n);
  std::vector<double> vof(n);
  std::vector<double> dfinvfsq(nrp1);

  gsl_matrix_view Q = gsl_matrix_view_array(sQ, m, m);
  
  gsl_vector_view Z = gsl_vector_view_array(sZ, m);  
  gsl_vector * Z_cp = gsl_vector_alloc(m);

  gsl_matrix * ZtZ = gsl_matrix_alloc(m, m);
  gsl_matrix_view maux1, maux2;
  maux1 = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), m, 1);
  gsl_vector_memcpy(Z_cp, &Z.vector);
  maux2 = gsl_matrix_view_array(gsl_vector_ptr(Z_cp, 0), 1, m);
  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix, 
    &maux2.matrix, 0.0, ZtZ);
  
  gsl_matrix * a_pred = gsl_matrix_alloc(n, m);
  std::vector<gsl_matrix*> P_pred(n);
    
  gsl_matrix * K = gsl_matrix_alloc(n, m);
  gsl_vector_view K_irow;
  
  std::vector<gsl_matrix*> L(n);
  
  gsl_vector_view Qdiag = gsl_matrix_diagonal(&Q.matrix);
  gsl_vector * Qdiag_msq = gsl_vector_alloc(m);
  gsl_vector_memcpy(Qdiag_msq, &Qdiag.vector);
  gsl_vector_mul(Qdiag_msq, &Qdiag.vector);
  gsl_vector_scale(Qdiag_msq, -1.0);
  
  std::vector<gsl_matrix*> da_pred(rp1);

  std::vector< std::vector<gsl_matrix*> > dP_pred(n, std::vector<gsl_matrix*>(rp1));

  std::vector<gsl_matrix*> dK(n);
  
  // filtering
  
  KF_deriv_aux_C(dim, sy, sZ, sT, sH, sR, sV, sQ, sa0, sP0, 
    &invf, &vof, dvof, &dfinvfsq, a_pred, &P_pred, K, 
    &L, &da_pred, &dP_pred, &dK);

  // state vector smoothing and disturbances smoothing

  gsl_matrix_view V = gsl_matrix_view_array(sV, ir, ir);  
  gsl_matrix_view R = gsl_matrix_view_array(sR, m, ir);
  
  gsl_vector_view vaux;
  gsl_vector *vaux2 = gsl_vector_alloc(m);
  
  gsl_matrix *Mmm = gsl_matrix_alloc(m, m);
  gsl_matrix *Mmm2 = gsl_matrix_alloc(m, m);
  gsl_matrix *Mrm = gsl_matrix_alloc(ir, m);

  gsl_vector_memcpy(Z_cp, &Z.vector);
  
  gsl_matrix *r0 = gsl_matrix_alloc(n + 1, m);
  gsl_vector_view r_row_t;
  gsl_vector_view r_row_tp1 = gsl_matrix_row(r0, n);
  gsl_vector_set_zero(&r_row_tp1.vector);

  std::vector<gsl_matrix*> N0(n + 1);
  N0.at(n) = gsl_matrix_calloc(m, m);
  gsl_vector_view Ndiag;

  gsl_vector *var_eps = gsl_vector_alloc(n);  
  msHsq = -1.0 * pow(*sH, 2);
  //vaux = gsl_vector_view_array(invf, n);
  vaux = gsl_vector_view_array(&invf[0], n);
  gsl_vector_set_all(var_eps, msHsq);
  gsl_vector_mul(var_eps, &vaux.vector);
  gsl_vector_add_constant(var_eps, *sH);

  gsl_vector *vr = gsl_vector_alloc(ir);

  gsl_matrix *dL = gsl_matrix_alloc(m, m);

  std::vector<gsl_matrix*> dr0(n + 1);
  dr0.at(n) = gsl_matrix_calloc(rp1, m);
  gsl_vector_view dr_row_t, dr_row_tp1;

  std::vector< std::vector<gsl_matrix*> > dN0(n + 1, std::vector<gsl_matrix*>(rp1));
  
  for (j = 0; j < rp1; j++)
  {
    (dN0.at(n)).at(j) = gsl_matrix_calloc(m, m);
  }

  for (i = n-1; i > -1; i--)
  {
    ip1 = i + 1;
    iaux = (i-1) * rp1m;
    irp1m = i * rp1m;

    if (i != n-1)  //the case i=n-1 was initialized above
      r_row_tp1 = gsl_matrix_row(r0, ip1);
    r_row_t = gsl_matrix_row(r0, i);

    gsl_blas_dgemv(CblasTrans, 1.0, L.at(i), &r_row_tp1.vector, 
      0.0, &r_row_t.vector);
    gsl_vector_memcpy(Z_cp, &Z.vector);
    gsl_vector_scale(Z_cp, vof.at(i));
    gsl_vector_add(&r_row_t.vector, Z_cp);

    gsl_vector_memcpy(vaux2, &r_row_tp1.vector);
    memcpy(&r[i * m], vaux2->data, msod);
    
    N0.at(i) = gsl_matrix_alloc(m, m);
    gsl_matrix_memcpy(N0.at(i), ZtZ);
    gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, L.at(i), N0.at(ip1), 0.0, Mmm);
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, L.at(i), invf.at(i), N0.at(i));
    
    vaux = gsl_matrix_diagonal(N0.at(ip1));
    gsl_vector_memcpy(vaux2, &vaux.vector);
    memcpy(&N[i * m], vaux2->data, msod);

    K_irow = gsl_matrix_row(K, i);
    gsl_blas_ddot(&K_irow.vector, &r_row_tp1.vector, &epshat[i]);
    epshat[i] -= vof.at(i);
    epshat[i] *= -*sH;

    maux1 = gsl_matrix_view_array(gsl_vector_ptr(&K_irow.vector, 0), 1, m);
    maux2 = gsl_matrix_view_array(gsl_vector_ptr(Z_cp, 0), 1, m);    
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix, N0.at(ip1),
      0.0, &maux2.matrix);
    vaux = gsl_vector_view_array(gsl_vector_ptr(var_eps, i), 1);
    gsl_blas_dgemv(CblasNoTrans, msHsq, &maux2.matrix, &K_irow.vector, 
      1.0, &vaux.vector);

    gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &V.matrix, &R.matrix,
      0.0, Mrm);
    gsl_blas_dgemv(CblasNoTrans, 1.0, Mrm, &r_row_tp1.vector,
      0.0, vr);
    memcpy(&etahat[i*ir], vr->data, irsod);

    Ndiag = gsl_matrix_diagonal(N0.at(ip1));
    gsl_vector_memcpy(Z_cp, &Ndiag.vector); 
    gsl_vector_mul(Z_cp, Qdiag_msq);
    gsl_vector_add(Z_cp, &Qdiag.vector);
    gsl_blas_dgemv(CblasTrans, 1.0, &R.matrix, Z_cp, 0.0, vr);    
    memcpy(&vareta[i*ir], vr->data, irsod);

    // derivatives 

    dr0.at(i) = gsl_matrix_alloc(rp1, m);
    
    for (j = 0; j < rp1; j++)
    {
      k = i + j * n;
      
      gsl_vector_memcpy(Z_cp, &Z.vector);
      gsl_vector_scale(Z_cp, dvof[k]);      

      vaux = gsl_matrix_row(dK.at(i), j);
      maux1 = gsl_matrix_view_array(gsl_vector_ptr(&vaux.vector, 0), m, 1);
      maux2 = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), 1, m);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &maux1.matrix, 
        &maux2.matrix, 0.0, dL);

      dr_row_t = gsl_matrix_row(dr0.at(i), j);
      dr_row_tp1 = gsl_matrix_row(dr0.at(ip1), j);
      gsl_blas_dgemv(CblasTrans, 1.0, dL, &r_row_tp1.vector, 0.0, &dr_row_t.vector);
      gsl_vector_add(&dr_row_t.vector, Z_cp);
      gsl_blas_dgemv(CblasTrans, 1.0, L.at(i), &dr_row_tp1.vector, 1.0, &dr_row_t.vector);

      (dN0.at(i)).at(j) = gsl_matrix_alloc(m, m);
      gsl_matrix_memcpy((dN0.at(i)).at(j), ZtZ);
      gsl_matrix_scale((dN0.at(i)).at(j), -1.0 * dfinvfsq.at(k));
      gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, dL, N0.at(ip1), 0.0, Mmm);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, L.at(i), 
        1.0, (dN0.at(i)).at(j));
      gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, L.at(i), 
        (dN0.at(ip1)).at(j), 0.0, Mmm);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, L.at(i), 
        1.0, (dN0.at(i)).at(j));
      gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, L.at(i), 
        N0.at(ip1), 0.0, Mmm);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, dL, 
        1.0, (dN0.at(i)).at(j));
      if (i != 0)
      {
        vaux = gsl_matrix_diagonal((dN0.at(i)).at(j));
        gsl_vector_memcpy(vaux2, &vaux.vector);
        memcpy(&dN[iaux + j * m], vaux2->data, msod);
      }

      vaux = gsl_matrix_row(da_pred.at(j), i);
      gsl_blas_dgemv(CblasNoTrans, 1.0, (dP_pred.at(i)).at(j) , &r_row_t.vector, 
        1.0, &vaux.vector);
      gsl_blas_dgemv(CblasNoTrans, 1.0, P_pred.at(i), &dr_row_t.vector, 
        1.0, &vaux.vector);
      gsl_vector_memcpy(vaux2, &vaux.vector);
      memcpy(&dahat[irp1m + j * m], vaux2->data, msod);

      gsl_matrix_memcpy(Mmm, (dP_pred.at(i)).at(j));
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, (dP_pred.at(i)).at(j), 
        N0.at(i), 0.0, Mmm2);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm2, P_pred.at(i),
        1.0, Mmm);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, P_pred.at(i), 
        (dN0.at(i)).at(j), 0.0, Mmm2);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm2, P_pred.at(i),
        1.0, Mmm);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, P_pred.at(i), 
        N0.at(i), 0.0, Mmm2);
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm2,
        (dP_pred.at(i)).at(j), 1.0, Mmm);
      
      gsl_matrix_mul_elements(Mmm, ZtZ);
      std::vector<double> vmm(Mmm->data, Mmm->data + m*m);
      dvareps[i*rp1 + j] = std::accumulate(vmm.begin(), vmm.end(), 0.0);

      gsl_matrix_free((dN0.at(ip1)).at(j));    
      gsl_matrix_free((dP_pred.at(i)).at(j));

    }

    if (i != 0)
    {
      memcpy(&dr[iaux], (dr0.at(i))->data, rp1msod);
    }

    gsl_matrix_free(dr0.at(ip1));
    
    gsl_matrix_free(dK.at(i));
    gsl_matrix_free(P_pred.at(i));
    gsl_matrix_free(L.at(i));
    gsl_matrix_free(N0.at(ip1));
  }

  gsl_matrix_free(N0.at(0));
  gsl_matrix_free(dr0.at(0));
  for (j = 0; j < rp1; j++)
  {
    gsl_matrix_free((dN0.at(0)).at(j));
    gsl_matrix_free(da_pred.at(j));
  }
  
  memcpy(&vareps[0], var_eps->data, nsod);
  
  gsl_matrix_free(Mmm);
  gsl_matrix_free(Mmm2);
  gsl_matrix_free(Mrm);
  
  gsl_matrix_free(r0);
  gsl_matrix_free(K);
  gsl_matrix_free(dL);
  
  gsl_matrix_free(a_pred);
  
  gsl_vector_free(Z_cp);
  gsl_matrix_free(ZtZ);
  gsl_vector_free(var_eps);
  gsl_vector_free(vr);
  gsl_vector_free(Qdiag_msq);
  gsl_vector_free(vaux2);
}}