// 骨架树变到下一骨架。所有骨架树的总数应该是catalan数C(2n,n)/(n+1)
// 对传统24点，n=3，骨架树共有C(6,3)/4 = 5种
bool go_next_state(node* p)
{
if (!p) return false;
// r_child^
if (go_next_state(p->r_child)) return true;
// l_child^, r_child=0
if (go_next_state(p->l_child))
{
node* r_child_new = initial_state(get_op_count(p->r_child));
delete (p->r_child);
p->r_child = r_child_new;
return true;
}
// l_child--=0 r_child++=0
int lcount = get_op_count(p->l_child);
int rcount = get_op_count(p->r_child);
if (lcount > 0)
{
node* l_child_new = initial_state(lcount-1);
node* r_child_new = initial_state(rcount+1);
delete (p->l_child);
delete (p->r_child);
p->l_child = l_child_new;
p->r_child = r_child_new;
return true;
}
return false;
}
void ExponentialSystem::analyticalSolution(const VectorXd& ts, MatrixXd& xs, MatrixXd& xds) const
{
int T = ts.size();
assert(T>0);
// Usually, we expect xs and xds to be of size T X dim(), so we resize to that. However, if the
// input matrices were of size dim() X T, we return the matrices of that size by doing a
// transposeInPlace at the end. That way, the user can also request dim() X T sized matrices.
bool caller_expects_transposed = (xs.rows()==dim() && xs.cols()==T);
// Prepare output arguments to be of right size (Eigen does nothing if already the right size)
xs.resize(T,dim());
xds.resize(T,dim());
VectorXd val_range = initial_state() - attractor_state();
VectorXd exp_term = -alpha_*ts/tau();
exp_term = exp_term.array().exp().transpose();
VectorXd pos_scale = exp_term;
VectorXd vel_scale = -(alpha_/tau()) * exp_term;
xs = val_range.transpose().replicate(T,1).array() * pos_scale.replicate(1,dim()).array();
xs += attractor_state().transpose().replicate(T,1);
xds = val_range.transpose().replicate(T,1).array() * vel_scale.replicate(1,dim()).array();
if (caller_expects_transposed)
{
xs.transposeInPlace();
xds.transposeInPlace();
}
}
PetscErrorCode cHamiltonianMatrix::timeEvolution(){
randV = gsl_vector_alloc(L);
randomPotential(randV);
// for (int ig = 0; ig < size; ++ig) { // I have to make sure the generated random number potential is the same across all CPUs.
// if (ig ==rank){
// cout << "rank " << rank << " has rand numbers:" << '\t';
// cout.precision(16);
// for (int i = 0; i < L; i++) {
// cout << gsl_vector_get(randV,i) << '\t';
// }
// cout << endl;
// }
// }
ierr = hamiltonianConstruction();CHKERRQ(ierr);
ierr = hamiltonianRescaling();CHKERRQ(ierr);
// initial state preperation
ierr = initial_state();CHKERRQ(ierr);
// %==============================================================
// % -------- time-dependent evolution --------------
// %-------------------------------------------------
// Kernal Polynomial Method
ierr = KernalPolynomialMethod();CHKERRQ(ierr);
return ierr;
}
void getDynamicalSystemsVector(vector<DynamicalSystem*>& dyn_systems)
{
// ExponentialSystem
double tau = 0.6; // Time constant
VectorXd initial_state(2); initial_state << 0.5, 1.0;
VectorXd attractor_state(2); attractor_state << 0.8, 0.1;
double alpha = 6.0; // Decay factor
dyn_systems.push_back(new ExponentialSystem(tau, initial_state, attractor_state, alpha));
// TimeSystem
dyn_systems.push_back(new TimeSystem(tau));
// TimeSystem (but counting down instead of up)
bool count_down = true;
dyn_systems.push_back(new TimeSystem(tau,count_down));
// SigmoidSystem
double max_rate = -20;
double inflection_point = tau*0.8;
dyn_systems.push_back(new SigmoidSystem(tau, initial_state, max_rate, inflection_point));
// SpringDamperSystem
alpha = 12.0;
dyn_systems.push_back(new SpringDamperSystem(tau, initial_state, attractor_state, alpha));
}
// 构造初始运算树，n为操作符的个数
node* initial_state(int n)
{
if (n == 0) return new node(nt_num);
node* p = new node(nt_op);
p->r_child = new node(nt_num);
p->l_child = initial_state(n-1);
return p;
}
// The inputs to ChaCha20 are:
// o A 256-bit key, treated as a concatenation of eight 32-bit little-endian integers.
// o A 96-bit nonce, treated as a concatenation of three 32-bit little-endian integers.
// o A 32-bit block count parameter, treated as a 32-bit little-endian integer.
// The output is 64 random-looking bytes.
void chacha20_block(uint8_t* out, const uint8_t* key, const uint8_t* nonce, uint32_t block_count)
{
state s;
initial_state(s, key, nonce, block_count);
block(s);
for (auto x : s) {
put_le_uint32(out, x);
out += 4;
}
}
int main (int argc, char * const argv[]) {
srand ( time(NULL) );
Optimization::SimulatedAnnealing::state_ptr initial_state(new BoardState(64));
std::pair<double, Optimization::SimulatedAnnealing::state_ptr> result = Optimization::SimulatedAnnealing::solve(initial_state, 30.0, 0.999, 1000000, 50, true);
std::cout << result.first << std::endl;
result.second->pretty_print();
return 0;
}
bool initializeFilterCallBack(laser_package::update_filter::Request &req, laser_package::update_filter::Response &res)
{
Eigen::MatrixXd initial_state(5,1);
initial_state << req.initial_x, req.initial_x_velocity, req.initial_y, req.initial_y_velocity, req.initial_turn_rate;
filter_1 = Filter(initial_state, req.sampling_interval, noise_data[FILTER_1], UM);
filter_2 = Filter(initial_state, req.sampling_interval, noise_data[FILTER_2], CT);
updateStateMessage(&filter_1, &state_msg_1, req);
updateStateMessage(&filter_2, &state_msg_2, req);
state_pub_1.publish(state_msg_1);
state_pub_2.publish(state_msg_2);
ROS_INFO("Filter initialized");
}
block_mapping_t find_mappings(operator_t const& op, bool diagonal_only = false) {
block_mapping_t mapping;
// Iteration over all initial basis states
for (index_t i = 0; i < tmp_state.size(); ++i) {
state_t initial_state = tmp_state;
initial_state(i) = amplitude_t(1.0);
auto i_subspace = subspaces.find_set(i);
state_t final_state = op(initial_state);
// Iterate over non-zero final amplitudes
foreach(final_state, [&](index_t f, amplitude_t amplitude) {
if (triqs::utility::is_zero(amplitude)) return;
auto f_subspace = subspaces.find_set(f);
if((!diagonal_only) || i_subspace==f_subspace)
mapping.insert(std::make_pair(representative_to_index[i_subspace],
representative_to_index[f_subspace]));
});
}
return mapping;
}
path_searcht::resultt path_searcht::operator()(
const goto_functionst &goto_functions)
{
#ifdef PATH_SYMEX_FORK
// Disable output because there is no meaningful way
// to write text when multiple path_search processes
// run concurrently. This could be remedied by piping
// to individual files or inter-process communication,
// a performance bottleneck, however.
*messaget::mstream.message_handler=NULL;
#endif
locst locs(ns);
var_mapt var_map(ns);
locs.build(goto_functions);
// this is the container for the history-forest
path_symex_historyt history;
queue.push_back(initial_state(var_map, locs, history));
// set up the statistics
number_of_paths=0;
number_of_instructions=0;
number_of_dropped_states=0;
number_of_VCCs=0;
number_of_VCCs_after_simplification=0;
number_of_failed_properties=0;
number_of_fast_forward_steps=0;
// stop the time
start_time=current_time();
initialize_property_map(goto_functions);
while(!queue.empty())
{
// Pick a state from the queue,
// according to some heuristic.
queuet::iterator state=pick_state();
// fast forwarding required?
if(state->is_lazy())
{
assert(state->is_executable());
assert(state->history.is_nil());
// keep allocated memory, this is faster than
// instantiating a new empty vector and map
history.clear();
var_map.clear();
state->history=path_symex_step_reft(history);
// restore all fields of a lazy state by symbolic
// execution along previously recorded branches
const queuet::size_type queue_size=queue.size();
do
{
number_of_fast_forward_steps++;
path_symex(*state, queue);
#ifdef PATH_SYMEX_OUTPUT
status() << "Fast forward thread " << state->get_current_thread()
<< "/" << state->threads.size()
<< " PC " << state->pc() << messaget::eom;
#endif
}
while(state->is_lazy() && state->is_executable());
assert(queue.size() == queue_size);
}
// TODO: check lazy states before fast forwarding, or perhaps it
// is better to even check before inserting into queue
if(drop_state(*state))
{
number_of_dropped_states++;
queue.erase(state);
continue;
}
if(!state->is_executable())
{
queue.erase(state);
continue;
}
// count only executable instructions
number_of_instructions++;
#ifdef PATH_SYMEX_OUTPUT
status() << "Queue " << queue.size()
<< " thread " << state->get_current_thread()
<< "/" << state->threads.size()
<< " PC " << state->pc() << messaget::eom;
#endif
// an error, possibly?
if(state->get_instruction()->is_assert())
{
if(show_vcc)
do_show_vcc(*state, ns);
else
{
check_assertion(*state, ns);
// all assertions failed?
if(number_of_failed_properties==property_map.size())
break;
}
}
#ifdef PATH_SYMEX_FORK
if(try_await())
{
debug() << "Child process has terminated "
"so exit parent" << messaget::eom;
break;
}
#endif
// execute and record whether a "branch" occurred
const queuet::size_type queue_size = queue.size();
path_symex(*state, queue);
assert(queue_size <= queue.size());
number_of_paths += (queue.size() - queue_size);
}
#ifdef PATH_SYMEX_FORK
int exit_status=await();
if(exit_status==0 && number_of_failed_properties!=0)
{
// the eldest child process (if any) reports found bugs
report_statistics();
return UNSAFE;
}
else
{
// either a child found and reported a bug or
// the parent's search partition is safe
switch (exit_status)
{
case 0: return SAFE;
case 10: return UNSAFE;
default: return ERROR;
}
}
#else
report_statistics();
return number_of_failed_properties==0?SAFE:UNSAFE;
#endif
}
void pins_model_init(model_t m) {
// create the LTS type LTSmin will generate
lts_type_t ltstype=lts_type_create();
// set the length of the state
lts_type_set_state_length(ltstype, state_length());
// add an "int" type for a state slot
int int_type = lts_type_add_type(ltstype, "int", NULL);
lts_type_set_format (ltstype, int_type, LTStypeDirect);
// add an "action" type for edge labels
int action_type = lts_type_add_type(ltstype, "action", NULL);
lts_type_set_format (ltstype, action_type, LTStypeEnum);
// add a "bool" type for state labels
int bool_type = lts_type_add_type (ltstype, LTSMIN_TYPE_BOOL, NULL);
lts_type_set_format(ltstype, bool_type, LTStypeEnum);
// set state name & type
for (int i=0; i < state_length(); ++i) {
char name[3]; sprintf(name, "%d", i);
lts_type_set_state_name(ltstype,i,name);
lts_type_set_state_typeno(ltstype,i,int_type);
}
// edge label types
lts_type_set_edge_label_count (ltstype, 1);
lts_type_set_edge_label_name(ltstype, 0, "action");
lts_type_set_edge_label_type(ltstype, 0, "action");
lts_type_set_edge_label_typeno(ltstype, 0, action_type);
// state label types
lts_type_set_state_label_count (ltstype, 1);
lts_type_set_state_label_name (ltstype, 0, "goal");
lts_type_set_state_label_typeno (ltstype, 0, bool_type);
// done with ltstype
lts_type_validate(ltstype);
// make sure to set the lts-type before anything else in the GB
GBsetLTStype(m, ltstype);
// setting all values for all non direct types
GBchunkPut(m, action_type, chunk_str("switch_a"));
GBchunkPut(m, action_type, chunk_str("switch_b"));
GBchunkPut(m, action_type, chunk_str("switch_c"));
GBchunkPut(m, action_type, chunk_str("switch_d"));
GBchunkPut(m, action_type, chunk_str("switch_ab"));
GBchunkPut(m, action_type, chunk_str("switch_ac"));
GBchunkPut(m, action_type, chunk_str("switch_ad"));
GBchunkPut(m, action_type, chunk_str("switch_bc"));
GBchunkPut(m, action_type, chunk_str("switch_bd"));
GBchunkPut(m, action_type, chunk_str("switch_cd"));
GBchunkPut(m, bool_type, chunk_str(LTSMIN_VALUE_BOOL_FALSE));
GBchunkPut(m, bool_type, chunk_str(LTSMIN_VALUE_BOOL_TRUE));
// set state variable values for initial state
GBsetInitialState(m, initial_state());
// set function pointer for the next-state function
GBsetNextStateLong(m, (next_method_grey_t) next_state);
// set function pointer for the label evaluation function
GBsetStateLabelLong(m, (get_label_method_t) state_label);
// create combined matrix
matrix_t *cm = malloc(sizeof(matrix_t));
dm_create(cm, group_count(), state_length());
// set the read dependency matrix
matrix_t *rm = malloc(sizeof(matrix_t));
dm_create(rm, group_count(), state_length());
for (int i = 0; i < group_count(); i++) {
for (int j = 0; j < state_length(); j++) {
if (read_matrix(i)[j]) {
dm_set(cm, i, j);
dm_set(rm, i, j);
}
}
}
GBsetDMInfoRead(m, rm);
// set the write dependency matrix
matrix_t *wm = malloc(sizeof(matrix_t));
dm_create(wm, group_count(), state_length());
for (int i = 0; i < group_count(); i++) {
for (int j = 0; j < state_length(); j++) {
if (write_matrix(i)[j]) {
dm_set(cm, i, j);
dm_set(wm, i, j);
}
}
}
GBsetDMInfoMustWrite(m, wm);
// set the combined matrix
GBsetDMInfo(m, cm);
// set the label dependency matrix
matrix_t *lm = malloc(sizeof(matrix_t));
dm_create(lm, label_count(), state_length());
for (int i = 0; i < label_count(); i++) {
for (int j = 0; j < state_length(); j++) {
if (label_matrix(i)[j]) dm_set(lm, i, j);
}
}
GBsetStateLabelInfo(m, lm);
}
DynamicalSystem* ExponentialSystem::clone(void) const
{
return new ExponentialSystem(tau(),initial_state(),attractor_state(),alpha_,name());
}
int main(int argc, char** argv)
{
bdd *c, *cp, *h, *hp, *t, *tp;
bdd I, T, R;
int n;
if(argc < 2)
{
printf("usage: %s N\n",argv[0]);
printf("\tN number of cyclers\n");
exit(1);
}
N = atoi(argv[1]);
if (N <= 0)
{
printf("The number of cyclers must more than zero\n");
exit(2);
}
bdd_init(100000, 10000);
bdd_setvarnum(N*6);
c = (bdd *)malloc(sizeof(bdd)*N);
cp = (bdd *)malloc(sizeof(bdd)*N);
t = (bdd *)malloc(sizeof(bdd)*N);
tp = (bdd *)malloc(sizeof(bdd)*N);
h = (bdd *)malloc(sizeof(bdd)*N);
hp = (bdd *)malloc(sizeof(bdd)*N);
normvar = (int *)malloc(sizeof(int)*N*3);
primvar = (int *)malloc(sizeof(int)*N*3);
for (n=0 ; n<N*3 ; n++)
{
normvar[n] = n*2;
primvar[n] = n*2+1;
}
normvarset = bdd_addref( bdd_makeset(normvar, N*3) );
pairs = bdd_newpair();
bdd_setpairs(pairs, primvar, normvar, N*3);
for (n=0 ; n<N ; n++)
{
c[n] = bdd_ithvar(n*6);
cp[n] = bdd_ithvar(n*6+1);
t[n] = bdd_ithvar(n*6+2);
tp[n] = bdd_ithvar(n*6+3);
h[n] = bdd_ithvar(n*6+4);
hp[n] = bdd_ithvar(n*6+5);
}
I = bdd_addref( initial_state(t,h,c) );
T = bdd_addref( transitions(t,tp,h,hp,c,cp) );
R = bdd_addref( reachable_states(I,T) );
/*if(has_deadlocks(R,T))
printf("Milner's Scheduler has deadlocks!\n"); */
printf("SatCount R = %.0f\n", bdd_satcount(R));
printf("Calc = %.0f\n", (double)N*pow(2.0,1.0+N)*pow(2.0,3.0*N));
bdd_done();
return 0;
}
path_searcht::resultt path_searcht::operator()(
const goto_functionst &goto_functions)
{
locst locs(ns);
var_mapt var_map(ns);
locs.build(goto_functions);
// this is the container for the history-forest
path_symex_historyt history;
queue.push_back(initial_state(var_map, locs, history));
// set up the statistics
number_of_dropped_states=0;
number_of_paths=0;
number_of_VCCs=0;
number_of_steps=0;
number_of_feasible_paths=0;
number_of_infeasible_paths=0;
number_of_VCCs_after_simplification=0;
number_of_failed_properties=0;
number_of_locs=locs.size();
// stop the time
start_time=current_time();
initialize_property_map(goto_functions);
while(!queue.empty())
{
number_of_steps++;
// Pick a state from the queue,
// according to some heuristic.
// The state moves to the head of the queue.
pick_state();
// move into temporary queue
queuet tmp_queue;
tmp_queue.splice(
tmp_queue.begin(), queue, queue.begin(), ++queue.begin());
try
{
statet &state=tmp_queue.front();
// record we have seen it
loc_data[state.get_pc().loc_number].visited=true;
debug() << "Loc: #" << state.get_pc().loc_number
<< ", queue: " << queue.size()
<< ", depth: " << state.get_depth();
for(const auto & s : queue)
debug() << ' ' << s.get_depth();
debug() << eom;
if(drop_state(state))
{
number_of_dropped_states++;
number_of_paths++;
continue;
}
if(!state.is_executable())
{
number_of_paths++;
continue;
}
if(eager_infeasibility &&
state.last_was_branch() &&
!is_feasible(state))
{
number_of_infeasible_paths++;
number_of_paths++;
continue;
}
if(number_of_steps%1000==0)
{
status() << "Queue " << queue.size()
<< " thread " << state.get_current_thread()
<< '/' << state.threads.size()
<< " PC " << state.pc() << messaget::eom;
}
// an error, possibly?
if(state.get_instruction()->is_assert())
{
if(show_vcc)
do_show_vcc(state);
else
{
check_assertion(state);
// all assertions failed?
if(number_of_failed_properties==property_map.size())
break;
}
}
// execute
path_symex(state, tmp_queue);
// put at head of main queue
queue.splice(queue.begin(), tmp_queue);
}
catch(const std::string &e)
{
error() << e << eom;
number_of_dropped_states++;
}
catch(const char *e)
{
error() << e << eom;
number_of_dropped_states++;
}
catch(int)
{
number_of_dropped_states++;
}
}
report_statistics();
return number_of_failed_properties==0?SAFE:UNSAFE;
}
int wmain(int argc, wchar_t* argv[])
{
std::wstring op_set = L"+-*/";
std::wstring all_bop = L"+-*/^_";
std::wstring all_uop = L"!";
bool show_help = false;
int rr_level = 1;
tp::cmdline_parser parser;
parser.add_option(L'p', L"opset", &op_set, true, &tp::cmdline_parser::cf_string);
parser.add_option(L'h', L"help", &show_help, false, &tp::cmdline_parser::cf_bool);
parser.add_option(L'r', L"rrlvl", &rr_level, true, &tp::cmdline_parser::cf_int);
parser.add_option(0, L"debug", &g_debug, false, &tp::cmdline_parser::cf_bool);
if (!parser.parse(argc, argv))
{
usage();
return 1;
}
if (show_help)
{
usage();
return 0;
}
if (op_set == L"all") op_set = all_bop + all_uop;
size_t c = parser.get_target_connt();
if (c < 3)
{
usage();
return 1;
}
// op_set -> bop+uop
std::wstring bop;
uti_list_t uop;
for (size_t i = 0; i < op_set.size(); i++)
{
if (all_bop.find(op_set[i]) != std::wstring::npos )
{
bop += op_set[i];
}
else if (all_uop.find(op_set[i]) != std::wstring::npos)
{
uti u = {op_set[i], 0};
for (i++; op_set[i] >= '0' && op_set[i] <= '9'; i++)
{
u.at_ub = u.at_ub * 10 + op_set[i] - '0';
}
if (u.at_ub == 0) u.at_ub = 1;
uop.push_back(u);
}
else
{
std::wcout << op_set[i] << L" is not a valid operator, run 'cal24 --help' to see valid operators" << std::endl;
return -2;
}
}
double d = _wtof(parser.get_target(c-1).c_str());
size_t num_count = c-1;
double* nums = new double[num_count];
for (size_t i = 0; i < num_count; i++)
{
nums[i] = _wtof(parser.get_target(i).c_str());
}
node* p = initial_state(num_count-1);
do
{
calc_on_exptree(p, bop.c_str(), uop, nums, num_count, d, rr_level);
}while (go_next_state(p));
delete p;
delete [] nums;
return 0;
}
void targetCallBack(const laser_package::state::ConstPtr& msg)
{
rho = msg->Measured_Rho;
theta = msg->Measured_Theta;
real_state(XI_INDEX) = msg->Real_X;
real_state(XI_DOT_INDEX) = msg->Real_X_Speed;
real_state(ETA_INDEX) = msg->Real_Y;
real_state(ETA_DOT_INDEX) = msg->Real_Y_Speed;
real_state(OMEGA_INDEX) = msg->Real_Omega;
z(RHO_INDEX) = rho;
z(THETA_INDEX) = theta;
if(msg_count>1)
{
update_time = msg->Time_Of_Measurement;
//extended kalman filter
ExtendedKF.updateFilter(z, update_time,real_state);
ExtendedKF.updateCovariance();
updateStateMessage(&ExtendedKF,msg);
extended_kalman_pub.publish(state_msg);
//regular kalman filter
KF.updateFilter(z, update_time, real_state);
KF.updateCovariance();
updateStateMessage(&KF,msg);
kalman_pub.publish(state_msg);
//imm
imm.update();
updateStateMessage(&imm,msg);
imm_pub.publish(state_msg);
}
else if(msg_count == 0)
{
first_xi = rho*cos(theta)+SENSOR_XI;
first_eta = rho*sin(theta)+SENSOR_ETA;
first_time = msg->Time_Of_Measurement;
msg_count++;
}
else if(msg_count==1)
{
second_time = msg->Time_Of_Measurement;
T = SAMPLING_INTERVAL;
initial_state(XI_INDEX) = rho*cos(theta)+SENSOR_XI;
initial_state(ETA_INDEX) = rho*sin(theta)+SENSOR_ETA;
initial_state(XI_DOT_INDEX) = (initial_state(XI_INDEX)-first_xi)/(T);
initial_state(ETA_DOT_INDEX) = (initial_state(ETA_INDEX)-first_eta)/(T);
initial_state(OMEGA_INDEX) = 0.0001;
//extended kalman filter
ExtendedKF = ExtendedKalmanFilter(initial_state,T , extended_kalman_noise_data,0.5,z);//instantiate Extended kalman filter
updateStateMessage(&ExtendedKF,msg);
extended_kalman_pub.publish(state_msg);
//regular kalman filter
KF = KalmanFilter(initial_state,T , kalman_noise_data, 0.5,z);//instantiate kalman filter
updateStateMessage(&KF,msg);
kalman_pub.publish(state_msg);
//imm
filters.push_back(&KF);
filters.push_back(&ExtendedKF);
imm = IMM(filters);//instantiate IMM with vector of filters
imm.update();
msg_count++;
}
//These are for when we want to publish to Rviz
target_point_msg.x = msg->Real_X;
target_point_msg.y = msg->Real_Y;
target_point_stamped_msg.point = target_point_msg;
target_point_stamped_msg.header.frame_id = "/my_frame";
//The below allows us to publish values so that Rviz can plot. You decide which points to plot from the target class.
target_real_pub.publish(target_point_stamped_msg);
}
