/// \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;
}
