예제 #1
0
파일: cal24.cpp 프로젝트: timepp/monalisa
// 骨架树变到下一骨架。所有骨架树的总数应该是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;
}
예제 #2
0
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();
  }
}
예제 #3
0
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));

}
예제 #5
0
파일: cal24.cpp 프로젝트: timepp/monalisa
// 构造初始运算树,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;
}
예제 #6
0
파일: chacha.cpp 프로젝트: mras0/funtls
// 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;
    }
}
예제 #7
0
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");
			
		}
예제 #9
0
  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;
 }
예제 #10
0
파일: path_search.cpp 프로젝트: Dthird/CBMC
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
}
예제 #11
0
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);

}
예제 #12
0
DynamicalSystem* ExponentialSystem::clone(void) const
{
  return new ExponentialSystem(tau(),initial_state(),attractor_state(),alpha_,name());
}
예제 #13
0
파일: cmilner.c 프로젝트: UGent-HES/javabdd
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;
}
예제 #14
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;
}
예제 #15
0
파일: cal24.cpp 프로젝트: timepp/monalisa
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);
		}