if(first_frame)WOLF_INFO("The first frame is ",first_frame," and it's currently estimated bias is: ",first_frame->getCaptureOf(sensor_ptr)->getStateBlock('I')->getState());
WOLF_INFO("Last frame is: ",last_frame_);
WOLF_INFO("Current state is: ",processor_motion->getState().vector("POV"));
WOLF_INFO("second_frame is: ",second_frame);
WOLF_INFO("Currently estimated bias is: ",C->getStateBlock('I')->getState());
WOLF_INFO("The current problem is:");
_problem_ptr->print(4);
C->setTimeStamp(t);
C->setData(data);
C->setDataCovariance(data_cov);
C->process();
inti=1;
for(t=t0+dt;t<=t0+2+dt/2;t+=dt){
WOLF_INFO("Starting iteration ",i," with timestamp ",t);
WOLF_INFO("0 - The first frame is ",first_frame->id()," and it's currently estimated bias is: \n",first_frame->getCaptureOf(sensor_ptr)->getStateBlock('I')->getState());
WOLF_INFO("its state vector is: \n",last_frame_->getStateVector());
WOLF_INFO_COND(second_frame,"The second frame has been created");
WOLF_INFO("1 - The last (current) frame is: ",last_frame_->id()," and it's state vector is: \n",last_frame_->getStateVector());
WOLF_INFO("2 - The current frame's estimated bias is: \n",last_frame_->getCaptureOf(sensor_ptr)->getStateBlock('I')->getState());
WOLF_INFO("3 - The current state is (from processor_motion): \n",processor_motion->getState().vector("POV"));
}
//WOLF_INFO("The current problem is:");
//_problem_ptr->print(4);
WOLF_INFO("Processing capture and solving... \n -----------------------------------------------------------------------------------");