Example #1
0
AD::SXMatrix CasadiSystem::cost_platt(const std::vector<AD::SXMatrix>& X, const std::vector<AD::SXMatrix>& U,
		const std::vector<AD::SXMatrix>& P) {
	std::vector<AD::SXMatrix> X_prop(T);
	X_prop[0] = X[0];
	for(int t=0; t < T-1; ++t) {
		X_prop[t+1] = this->dynfunc(X[t], U[t]);
	}

	std::vector<AD::SXMatrix> H(M, AD::SXMatrix(T*(Z_DIM),1));
	for(int m=0; m < M; ++m) {
		int index = 0;
		for(int t=0; t < T; ++t) {
			AD::SXMatrix Hm = this->obsfunc(X_prop[t], P[m]);
			for(int i=0; i < Z_DIM; ++i) {
				H[m](index++,0) = Hm(i,0);
			}
		}
	}

	AD::SXMatrix platt(1,1);
	for(int m=1; m < M; ++m) {
		AD::SXMatrix diff = H[m] - H[0];
		platt(0,0) += (1/float(M-1))*exp(-mul(trans(diff), diff));
	}

	return platt;
}
Example #2
0
AD::SXMatrix CasadiSystem::cost_entropy(const std::vector<AD::SXMatrix>& X, const std::vector<AD::SXMatrix>& U,
										const std::vector<AD::SXMatrix>& P) {
	AD::SXMatrix entropy(1,1);

	std::vector<AD::SXMatrix> X_prop(T);
	std::vector<std::vector<AD::SXMatrix> > H(T, std::vector<AD::SXMatrix>(M));
	std::vector<std::vector<std::vector<AD::SXMatrix> > > GL_H(T, std::vector<std::vector<AD::SXMatrix> >(M, std::vector<AD::SXMatrix>(M)));
	for(int t=0; t < T-1; ++t) {
		X_prop[t+1] = this->dynfunc(X[t], U[t]);
		for(int m=0; m < M; ++m) {
			H[t+1][m] = this->obsfunc(X_prop[t+1], P[m]);
		}
	}

	for(int t=1; t < T; ++t) {
		for(int m=0; m < M; ++m) {
			for(int p=m; p < M; ++p) {
				GL_H[t][m][p] = this->gauss_likelihood(H[t][m] - H[t][p]);
			}
		}
	}

	std::vector<std::vector<AD::SXMatrix> > W(T, std::vector<AD::SXMatrix>(M, AD::SXMatrix(1,1)));
	for(int m=0; m < M; ++m) { W[0][m](0,0) = 1/float(M); }
	for(int t=1; t < T; ++t) {

		AD::SXMatrix W_sum(1,1);
		for(int m=0; m < M; ++m) {
			for(int p=0; p < M; ++p) {
				int a = MIN(m,p);
				int b = MAX(m,p);
				W[t][m] += GL_H[t][a][b];
			}
			W_sum += W[t][m];
		}
		for(int m=0; m < M; ++m) { W[t][m] = W[t][m] / W_sum; }

		// use skoglar version
		AD::SXMatrix entropy_t(1,1);
		for(int m=0; m < M; ++m) {
			entropy_t += -W[t][m]*log(W[t][m]);
		}

		// simplifies because zero particle dynamics
		for(int m=0; m < M; ++m) {
			entropy_t += -W[t][m]*log(W[t-1][m]);
		}

		AD::SXMatrix sum_cross_time_weights(1,1);
		for(int m=0; m < M; ++m) {
			sum_cross_time_weights += W[t-1][m]*W[t][m];
		}
		entropy_t += log(sum_cross_time_weights);

		entropy += entropy_t;
	}

//	for(int t=0; t < T-2; ++t) {
//		entropy += Constants::alpha_control_smooth*(mul(trans(U[t+1]-U[t]),(U[t+1]-U[t])));
//	}
//
//	for(int t=0; t < T-1; ++t) {
//		entropy += Constants::alpha_control_norm*mul(trans(U[t]),U[t]);
//	}
//
//	for(int t=1; t < T; ++t) {
//		for(int n=0; n < N; ++n) {
//			AD::SXMatrix x_t_n = X_prop[t](AD::Slice(n*X_DIM,(n+1)*X_DIM));
//			for(int n_other=0; n_other < N; n_other++) {
//				AD::SXMatrix x_t_n_other = X_prop[t](AD::Slice(n_other*X_DIM,(n_other+1)*X_DIM));
//				entropy += Constants::alpha_separation*mul(trans(x_t_n-x_t_n_other), x_t_n-x_t_n_other);
//			}
//		}
//	}

	return entropy;
}
Example #3
0
inline
bool
de_prmm_int(arma::vec& init_out_vals, std::function<double (const arma::vec& vals_inp, arma::vec* grad_out, void* opt_data)> opt_objfn, void* opt_data, algo_settings_t* settings_inp)
{
    bool success = false;

    const size_t n_vals = init_out_vals.n_elem;

    //
    // DE settings

    algo_settings_t settings;

    if (settings_inp) {
        settings = *settings_inp;
    }

    const uint_t conv_failure_switch = settings.conv_failure_switch;
    const double err_tol = settings.err_tol;

    size_t n_pop = settings.de_n_pop;
    // const size_t check_freq = settings.de_check_freq;

    const double par_initial_F = settings.de_par_F;
    const double par_initial_CR = settings.de_par_CR;

    const arma::vec par_initial_lb = (settings.de_initial_lb.n_elem == n_vals) ? settings.de_initial_lb : init_out_vals - 0.5;
    const arma::vec par_initial_ub = (settings.de_initial_ub.n_elem == n_vals) ? settings.de_initial_ub : init_out_vals + 0.5;

    const double F_l = settings.de_par_F_l;
    const double F_u = settings.de_par_F_u;
    const double tau_F  = settings.de_par_tau_F;
    const double tau_CR = settings.de_par_tau_CR;

    arma::vec F_vec(n_pop), CR_vec(n_pop);
    F_vec.fill(par_initial_F);
    CR_vec.fill(par_initial_CR);

    const uint_t max_fn_eval = settings.de_max_fn_eval;
    const uint_t pmax = settings.de_pmax;
    const size_t n_pop_best = settings.de_n_pop_best;

    const double d_eps = 0.5;

    size_t n_gen = std::ceil(max_fn_eval / (pmax*n_pop));

    const bool vals_bound = settings.vals_bound;
    
    const arma::vec lower_bounds = settings.lower_bounds;
    const arma::vec upper_bounds = settings.upper_bounds;

    const arma::uvec bounds_type = determine_bounds_type(vals_bound, n_vals, lower_bounds, upper_bounds);

    // lambda function for box constraints

    std::function<double (const arma::vec& vals_inp, arma::vec* grad_out, void* box_data)> box_objfn \
    = [opt_objfn, vals_bound, bounds_type, lower_bounds, upper_bounds] (const arma::vec& vals_inp, arma::vec* grad_out, void* opt_data) \
    -> double 
    {
        if (vals_bound)
        {
            arma::vec vals_inv_trans = inv_transform(vals_inp, bounds_type, lower_bounds, upper_bounds);
            
            return opt_objfn(vals_inv_trans,nullptr,opt_data);
        }
        else
        {
            return opt_objfn(vals_inp,nullptr,opt_data);
        }
    };

    //
    // setup

    arma::vec objfn_vals(n_pop);
    arma::mat X(n_pop,n_vals), X_next(n_pop,n_vals);

#ifdef OPTIM_USE_OMP
    #pragma omp parallel for
#endif
    for (size_t i=0; i < n_pop; i++) 
    {
        X_next.row(i) = par_initial_lb.t() + (par_initial_ub.t() - par_initial_lb.t())%arma::randu(1,n_vals);

        double prop_objfn_val = opt_objfn(X_next.row(i).t(),nullptr,opt_data);

        if (!std::isfinite(prop_objfn_val)) {
            prop_objfn_val = inf;
        }
        
        objfn_vals(i) = prop_objfn_val;

        if (vals_bound) {
            X_next.row(i) = arma::trans( transform(X_next.row(i).t(), bounds_type, lower_bounds, upper_bounds) );
        }
    }

    double best_objfn_val_running = objfn_vals.min();
    // double best_objfn_val_check   = best_objfn_val_running;

    double best_val_main = best_objfn_val_running;
    double best_val_best = best_objfn_val_running;

    arma::rowvec best_sol_running = X_next.row( objfn_vals.index_min() );
    arma::rowvec best_vec_main = best_sol_running;
    arma::rowvec best_vec_best = best_sol_running;

    arma::rowvec xchg_vec = best_sol_running;

    //

    uint_t n_reset = 1;
    uint_t iter = 0;
    double err = 2*err_tol;

    while (err > err_tol && iter < n_gen + 1)
    {
        iter++;

        //
        // population reduction step

        if (iter == n_gen && n_reset < 4)
        {
            size_t n_pop_temp = n_pop/2;

            arma::vec objfn_vals_reset(n_pop_temp);
            arma::mat X_reset(n_pop_temp,n_vals);

#ifdef OPTIM_USE_OMP
            #pragma omp parallel for
#endif
            for (size_t j=0; j < n_pop_temp; j++) 
            {
                if (objfn_vals(j) < objfn_vals(j + n_pop_temp)) 
                {
                    X_reset.row(j) = X_next.row(j);
                    objfn_vals_reset(j) = objfn_vals(j);
                }
                else
                {
                    X_reset.row(j) = X_next.row(j + n_pop_temp);
                    objfn_vals_reset(j) = objfn_vals(j + n_pop_temp);
                }
            }

            objfn_vals = objfn_vals_reset;
            X_next = X_reset;

            n_pop /= 2;
            n_gen *= 2;

            iter = 1;
            n_reset++;
        }

        X = X_next;

        //
        // first population: n_pop - n_pop_best
        

#ifdef OPTIM_USE_OMP
        #pragma omp parallel for
#endif
        for (size_t i=0; i < n_pop - n_pop_best; i++)
        {
            arma::vec rand_pars = arma::randu(4);

            if (rand_pars(0) < tau_F) {
                F_vec(i) = F_l + (F_u-F_l)*rand_pars(1);
            }

            if (rand_pars(2) < tau_CR) {
                CR_vec(i) = rand_pars(3);
            }

            //

            uint_t c_1, c_2, c_3;

            do {
                c_1 = arma::as_scalar(arma::randi(1, arma::distr_param(0, n_pop-1)));
            } while(c_1==i);

            do {
                c_2 = arma::as_scalar(arma::randi(1, arma::distr_param(0, n_pop-1)));
            } while(c_2==i || c_2==c_1);

            do {
                c_3 = arma::as_scalar(arma::randi(1, arma::distr_param(0, n_pop-1)));
            } while(c_3==i || c_3==c_1 || c_3==c_2);

            //

            size_t j = arma::as_scalar(arma::randi(1, arma::distr_param(0, n_vals-1)));

            arma::vec rand_unif = arma::randu(n_vals);
            arma::rowvec X_prop(n_vals);

            for (size_t k=0; k < n_vals; k++)
            {
                if ( rand_unif(k) < CR_vec(i) || k == j )
                {
                    double r_s = arma::as_scalar(arma::randu(1));

                    if ( r_s < 0.75 || n_pop >= 100 ) {
                        X_prop(k) = X(c_3,k) + F_vec(i)*(X(c_1,k) - X(c_2,k));
                    } else {
                        X_prop(k) = best_vec_main(k) + F_vec(i)*(X(c_1,k) - X(c_2,k));
                    }
                }
                else
                {
                    X_prop(k) = X(i,k);
                }
            }

            //

            double prop_objfn_val = box_objfn(X_prop.t(),nullptr,opt_data);
            
            if (prop_objfn_val <= objfn_vals(i))
            {
                X_next.row(i) = X_prop;
                objfn_vals(i) = prop_objfn_val;
            }
            else
            {
                X_next.row(i) = X.row(i);
            }
        }

        best_val_main = objfn_vals.rows(0,n_pop - n_pop_best - 1).min();
        best_vec_main = X_next.rows(0,n_pop - n_pop_best - 1).row( objfn_vals.rows(0,n_pop - n_pop_best - 1).index_min() );

        if (best_val_main < best_val_best) {
            xchg_vec = best_vec_main;
        }

        //
        // second population

        for (size_t i = n_pop - n_pop_best; i < n_pop; i++)
        {
            arma::vec rand_pars = arma::randu(4);

            if (rand_pars(0) < tau_F) {
                F_vec(i) = F_l + (F_u-F_l)*rand_pars(1);
            }

            if (rand_pars(2) < tau_CR) {
                CR_vec(i) = rand_pars(3);
            }

            //

            uint_t c_1, c_2;

            do {
                c_1 = arma::as_scalar(arma::randi(1, arma::distr_param(0, n_pop-1)));
            } while(c_1==i);

            do {
                c_2 = arma::as_scalar(arma::randi(1, arma::distr_param(0, n_pop-1)));
            } while(c_2==i || c_2==c_1);

            //

            size_t j = arma::as_scalar(arma::randi(1, arma::distr_param(0, n_vals-1)));

            arma::vec rand_unif = arma::randu(n_vals);
            arma::rowvec X_prop(n_vals);

            for (size_t k=0; k < n_vals; k++) {
                if ( rand_unif(k) < CR_vec(i) || k == j ) {
                    X_prop(k) = best_vec_best(k) + F_vec(i)*(X(c_1,k) - X(c_2,k));
                } else {
                    X_prop(k) = X(i,k);
                }
            }

            //

            double prop_objfn_val = box_objfn(X_prop.t(),nullptr,opt_data);
            
            if (prop_objfn_val <= objfn_vals(i))
            {
                X_next.row(i) = X_prop;
                objfn_vals(i) = prop_objfn_val;
            }
            else
            {
                X_next.row(i) = X.row(i);
            }
        }

        best_val_best = objfn_vals.rows(n_pop - n_pop_best, n_pop - 1).min();
        best_vec_best = X_next.rows(n_pop - n_pop_best, n_pop - 1).row( objfn_vals.rows(n_pop - n_pop_best, n_pop - 1).index_min() );

        if (best_val_best < best_val_main)
        {
            double the_sum = 0;

            for (size_t j=0; j < n_vals; j++) 
            {
                double min_val = X.col(j).min();

                the_sum += (best_vec_best(j) - min_val) / (xchg_vec(j) - min_val);
            }

            the_sum /= static_cast<double>(n_vals);

            if (std::abs(the_sum - 1.0) > d_eps) {
                best_vec_main = best_vec_best;
            } else {
                best_vec_best = best_vec_main;
            }
        }
        else
        {
            best_vec_best = best_vec_main;
        }

        //
        // assign running global minimum

        if (objfn_vals.min() < best_objfn_val_running)
        {
            best_objfn_val_running = objfn_vals.min();
            best_sol_running = X_next.row( objfn_vals.index_min() );
        }

        // if (iter%check_freq == 0) {
        //     // err = std::abs(objfn_vals.min() - best_objfn_val) / (std::abs(best_objfn_val) + 1E-08);
        //     err = std::abs(best_objfn_val_running - best_objfn_val_check);
        //     // best_objfn_val_check = objfn_vals.min();
        //     if (best_objfn_val_running < best_objfn_val_check) {
        //         best_objfn_val_check = best_objfn_val_running;
        //     }
        // }
    }

    //

    if (vals_bound) {
        best_sol_running = arma::trans( inv_transform(best_sol_running.t(), bounds_type, lower_bounds, upper_bounds) );
    }

    error_reporting(init_out_vals,best_sol_running.t(),opt_objfn,opt_data,success,err,err_tol,iter,n_gen,conv_failure_switch,settings_inp);

    //

    return true;
}