int main(int argc, char **argv)
{
int fileDescriptor;
parse_opts(argc,argv);
if (mode<0 || mode>3) {
printf("Unsupported measurement mode %d!\n",mode);
print_usage(argv[0]);
exit (1);
}
BMP085 *sensor;
sensor = (BMP085 *) malloc(sizeof(BMP085));
sensor->i2cAddress = i2cAddress;
sensor->oss = mode;
fileDescriptor = open(device, O_RDWR);
if (fileDescriptor<0) {
printf("\nFailed to open I2C port! Did you sudo?\n");
print_usage(argv[0]);
exit(1);
}
if (ioctl(fileDescriptor, I2C_SLAVE, sensor->i2cAddress)<0) {
printf("Failed to select i2c device!\n");
exit(1);
}
if ( ! readCalibrationTable(fileDescriptor,sensor)) {
printf("Failed to read calibration table!\n");
exit(1);
}
if (printSensorCalibrationTable) {
printf("Table of calibration coeficients:\n");
printCalibrationTable(sensor);
}
makeMeasurement(fileDescriptor,sensor);
if (measurePressure)
printf(" Pressure: % 11.4f hPa\t=% 5.2f inch of mercury\n",
((float)sensor->pressure/ 100.0),((float)sensor->pressure / 100.0) * 0.02953);
if (measureTemperature)
printf("Temperature: % 11.4f C\t=% 5.2f F\n",sensor->temperature, (sensor->temperature * 1.8) + 32);
if (Altitude)
printf(" Altitude: % 11.4f m\t=% .2f f\n",44330.0 * (1.0 - pow(sensor->pressure / seaLevelPressure, 0.1903)),
44330.0 * (1.0 - pow(sensor->pressure / seaLevelPressure, 0.1903)) * 3.2808);
free(sensor);
close(fileDescriptor);
return 0;
}
void EKalman::runKalmanUp(Detections& det, int frame, int t, Matrix<double>& allXnew, Vector<FrameInlier>& Idx,
Vector<double>& R, Vector<double>& Vel, Volume<double>& hMean, Vector<Matrix<double> >& stateCovMats, Volume<double>& colHistInit,
Vector<Volume<double> >& colHists, double yPosOfStartingPoint, Vector<double>& bbox)
{
m_bbox = bbox;
m_Up = true;
int tLastSupport = 0;
m_colHist = colHistInit;
m_yPos.pushBack(m_xpost(0));
m_yPos.pushBack(yPosOfStartingPoint);
m_yPos.pushBack(m_xpost(1));
for(int i = frame; i < t; i++)
{
if(tLastSupport > Globals::maxHoleLen) break;
predict();
m_measurement_found = findObservation(det, i, i,i);
if(m_measurement_found)
{
m_measurement = makeMeasurement();
update();
saveData(i+1);
tLastSupport = 0;
}
else
{
update();
saveData(i+1);
tLastSupport += 1;
}
}
Vel = m_Vel;
R = m_Rot;
Idx = m_Idx;
allXnew = Matrix<double>(m_allXnew);
hMean = m_colHist;
colHists = m_colHists;
stateCovMats = m_CovMats;
}
void EKalman::runKalmanDown(Detections& det, int frame, int pointPos, int t, Matrix<double>& allXnew, Vector<FrameInlier>& Idx,
Vector<double>& R, Vector<double>& Vel, Volume<double>& hMean, Vector<Matrix<double> >& stateCovMats,
Vector<Volume<double> >& colHists)
{
m_Up = false;
det.getColorHist(frame, pointPos, m_colHist);
det.getBBox(frame, pointPos, m_bbox);
Matrix<double> copyInitStateUnc = m_Ppost;
Vector<double> copyInitState = m_xpost;
//////////////////////////////////////////////////////////////////////
Matrix<double> covMatrix;
det.get3Dcovmatrix(frame, pointPos, covMatrix);
Vector<double> startingDet;
det.getPos3D(frame, pointPos, startingDet);
m_R.fill(0.0);
m_R(0,0) = covMatrix(0,0);
m_R(1,1) = covMatrix(2,2);
m_R(2,2) = 0.2*0.2;
m_R(3,3) = 0.2*0.2;
int tLastSupport = 0;
m_yPos.pushBack(m_xpost(0));
m_yPos.pushBack(startingDet(1));
m_yPos.pushBack(m_xpost(1));
int accepted_frames_without_det = 20;
for(int i = frame+1; i > t; i--)
{
if(tLastSupport > accepted_frames_without_det)
break;
predict();
m_measurement_found = findObservation(det, i, pointPos, frame);
if(m_measurement_found)
{
m_measurement = makeMeasurement();
update();
if(i == frame+1)
{
m_Ppost = copyInitStateUnc;
m_xpost = copyInitState;
}
saveData(i-1);
tLastSupport = 0;
pointPos = m_Idx(m_Idx.getSize()-1).getInlier()(0);
}
else
{
update();
m_xpost(3) *= 0.8;
saveData(i-1);
tLastSupport += 1;
}
}
if(m_Rot.getSize() > 1) {
// As this is kalman-DOWN, we start with an unassociated detection (InitState)
// The position is fine, but we fix the rotation by copying the one from the successor state
m_Rot(0) = m_Rot(1);
}
Vel = m_Vel;
turnRotations(R, m_Rot);
Idx = m_Idx;
allXnew = Matrix<double>(m_allXnew);
hMean = m_colHist;
colHists = m_colHists;
stateCovMats = m_CovMats;
}
int StateEstimatorKinematic::run_state_est_lin_task() {
if (first_time) {
//initKF(Eigen::Vector3d(base_state.x[1],base_state.x[2],base_state.x[3]), Eigen::Vector3d(base_state.xd[1],base_state.xd[2],base_state.xd[3]));
//initKF(Eigen::Vector3d(0,0,0), Eigen::Vector3d(0,0,0));
first_time = false;
buffer_first_time = true;
contact_change_flag = true;
sek.initKF(Eigen::Vector3d(0,0,0), Eigen::Vector3d(0,0,0));
computeAnkleRelated();
contact_change_flag = true;
set_hack_footpos();
}
if (initializing_q > 0) {
initializing_q--;
Eigen::Matrix<double,3,1> current_imu_linear_acceleration;
current_imu_linear_acceleration[0] = misc_sensor[B_XACC_IMU];
current_imu_linear_acceleration[1] = misc_sensor[B_YACC_IMU];
current_imu_linear_acceleration[2] = misc_sensor[B_ZACC_IMU];
init_gravity_vector = 0.995 * init_gravity_vector + 0.005 *( _imu_quat_offset.conjugate()*current_imu_linear_acceleration);
if(initializing_q == 1) {
_q.setFromTwoVectors(init_gravity_vector,Eigen::Vector3d(0, 0, 1));
std::cout << "Done Initializing Q" <<std::endl;
std::cout << "Final vector = " <<std::endl;
std::cout << _imu_quat_offset*init_gravity_vector;
std::cout<<std::endl<<std::endl;
_gravity = -(_q.matrix()*(init_gravity_vector));
std::cout << "Final gravity = " <<std::endl;
std::cout << _gravity;
std::cout<<std::endl<<std::endl;
}
if(initializing_q ==0) {
computeAnkleRelated();
contact_change_flag = true;
set_hack_footpos();
}
_x[0] = 0;
_x[1] = 0;
_x[2] = 0;
_x[3] = 0;
_x[4] = 0;
_x[5] = 0;
return TRUE;
}
// static int printcounter = 1000;
// printcounter++;
// if (printcounter > 1000){
// printcounter = 0;
//
// std::cout<<"innov === "<<_innov[0] <<"\t"<<_innov[1]<<"\t"<<_innov[2] <<"\t" <<_innov[3] <<"\t"<<_innov[4]<<"\t"<<_innov[5];
// std::cout<<std::endl;
// std::cout<<"z === "<<_z[0] <<"\t"<<_z[1]<<"\t"<<_z[2] <<"\t" <<_z[3] <<"\t"<<_z[4]<<"\t"<<_z[5];
// std::cout<<std::endl;
// std::cout<<"y === "<<_y[0] <<" "<<_y[1]<<" "<<_y[2] <<"\t" <<_y[3] <<" "<<_y[4]<<" "<<_y[5];
// std::cout<<std::endl;
//// std::cout<<"err: "<<base_state.x[1] - _x[0] <<" "<<base_state.x[2] - _x[1]<<" "<<base_state.x[3] - _x[2] <<
//// "\t" <<base_state.xd[1] - _x[3] <<" "<<base_state.xd[2] - _x[4]<<" "<<base_state.xd[3] - _x[5];
// std::cout<<std::endl;
// std::cout<<std::endl;
// }
Eigen::Matrix<double,3,1> imu_angular_velocity;
imu_angular_velocity[0] = misc_sensor[B_AD_A_IMU];
imu_angular_velocity[1] = misc_sensor[B_AD_B_IMU];
imu_angular_velocity[2] = misc_sensor[B_AD_G_IMU];
Eigen::Matrix<double,3,1> imu_linear_acceleration;
imu_linear_acceleration[0] = misc_sensor[B_XACC_IMU];
imu_linear_acceleration[1] = misc_sensor[B_YACC_IMU];
imu_linear_acceleration[2] = misc_sensor[B_ZACC_IMU];
Eigen::Matrix<double, 50, 1> joints;
for(int x =0; x < 50; x++) joints[x] = joint_state[x+1].th;
Eigen::Matrix<double, 50, 1> jointsd;
for(int x =0; x < 50; x++) jointsd[x] = joint_state[x+1].thd;
imu_angular_velocity = _imu_quat_offset.normalized().conjugate().toRotationMatrix()*imu_angular_velocity;
integrate_angular_velocity(imu_angular_velocity);
makeInputs(_q, imu_angular_velocity, imu_linear_acceleration, joints, jointsd);
setContactState(2, -1);
makeMeasurement(misc_sensor[L_CFx], misc_sensor[R_CFx]);
filterOneTimeStep_ss();
return TRUE;
}
