/// \brief Unscented transform of process Sigma points void UnscentedKalmanFilter::utf(vnl_matrix<double> X, vnl_vector<double> u, vnl_vector<double> &y, vnl_matrix<double> &Y, vnl_matrix<double> &P, vnl_matrix<double> &Y1) { // determine number of sigma points unsigned int L = X.cols(); // zero output matrices y.fill(0.0); Y.fill(0.0); // transform the sigma points and put them as columns in a matrix Y for( int k = 0; k < L; k++ ) { vnl_vector<double> xk = X.get_column(k); vnl_vector<double> yk(N); f(xk,u,yk); Y.set_column(k,yk); // add each transformed point to the weighted mean y = y + Wm.get(0,k)*yk; } // create a matrix with each column being the weighted mean vnl_matrix<double> Ymean(N,L); for( int k = 0; k < L; k++ ) Ymean.set_column(k,y); // set the matrix of difference vectors Y1 = Y-Ymean; // calculate the covariance matrix output vnl_matrix<double> WC(L,L,0.0); WC.set_diagonal(Wc.get_row(0)); P = Y1*WC*Y1.transpose(); }
vnl_matrix<double> MCLR_SM::Get_F_Matrix(vnl_matrix<double> data_bias,vnl_matrix<double> w_temp) { vnl_matrix<double> epow_matrix = w_temp.transpose()*data_bias; vnl_matrix<double> temp_f; temp_f.set_size(epow_matrix.rows(),epow_matrix.cols()); for(int i=0;i<epow_matrix.rows();++i) { for(int j=0;j<epow_matrix.cols();++j) { temp_f(i,j) = exp(epow_matrix(i,j)); } } return temp_f; }
void MCLR_SM::Get_Hessian(vnl_matrix<double> data_with_bias) { vnl_matrix<double> f; f = Get_F_Matrix(data_with_bias,m.w); f = Normalize_F_Sum(f); vnl_matrix<double> temp_hessian; // temporary Hessian Matrix temp_hessian.set_size((no_of_features+1)*(no_of_classes),(no_of_features+1)*(no_of_classes)); for(int i = 1; i<=no_of_classes ;++i) { vnl_vector<double> ith_row = f.get_row(i-1); for(int j = 0; j< x.cols() ; ++j) { ith_row(j) = ith_row(j)*(1-ith_row(j)); } vnl_diag_matrix<double> diagonal_matrix(ith_row); vnl_matrix<double> temp_matrix = -data_with_bias*diagonal_matrix*data_with_bias.transpose(); temp_hessian.update(temp_matrix,(i-1)*(no_of_features+1),(i-1)*(no_of_features+1)); // //for(int k = 0; k< class_vector.size() ; ++k) // std::cout<<class_vector(k)<<"--------------"<<std::endl; vnl_vector<int> all_class_but_i; all_class_but_i.set_size(class_vector.size()-1); int counter = 0; for(int k = 0; k< class_vector.size() ; ++k) { if(class_vector(k)!=i) { all_class_but_i.put(counter,class_vector(k)); counter ++; } } //for(int k = 0; k< class_vector.size() ; ++k) // std::cout<<class_vector(k)<<"--------------"<<std::endl; for(int k = 0; k< all_class_but_i.size() ; ++k) { ith_row = f.get_row(i-1); vnl_vector<double> kth_row = f.get_row(all_class_but_i(k)-1); for(int j = 0; j< x.cols() ; ++j) { //f(i,:).*(-f(k,:)) ith_row(j) = ith_row(j)*(-kth_row(j)); } vnl_diag_matrix<double> diagonal_matrix(ith_row); vnl_matrix<double> temp_matrix = -data_with_bias*diagonal_matrix*data_with_bias.transpose(); temp_hessian.update(temp_matrix,(i-1)*(no_of_features+1),(all_class_but_i(k)-1)*(no_of_features+1)); } } //hessian = hessian - diag(C*(delta./(sqrt(w(:).^2+delta)).^3)); //Need to vectorize w vnl_vector<double> w_column_ordered = Column_Order_Matrix(m.w); int count = 0; for(int i=0;i<m.w.rows();++i) { for(int j=0;j<m.w.cols();++j) { w_column_ordered(count) = (m.sparsity_control)*(delta/pow(sqrt(pow(w_column_ordered(count),2)+delta),3)); count++; } } vnl_diag_matrix<double> diagonal_matrix(w_column_ordered); temp_hessian = temp_hessian - diagonal_matrix; //temp_hessian.extract(hessian,no_of_features+2,no_of_features+2); hessian = temp_hessian; //std::cout<<"------------------------------------------------------"<<std::endl; //std::cout<<hessian.get_row(0)<<std::endl; //std::cout<<"------------------------------------------------------"<<std::endl; //std::cout<<hessian.get_row(115)<<std::endl; //std::cout<<"------------------------------------------------------"<<std::endl; }