Open Needrom opened 1 year ago
Hi Needrom, without complete code, it is hard to help you out... Did you start from one of the BFL examples? Which 'main' did you use?
Hi Needrom, without complete code, it is hard to help you out... Did you start from one of the BFL examples? Which 'main' did you use?
yes. of course I start from the EKF example. I change it to the 6 state equation. I can show you the main part of my code.
void SysUncertainty(){
ColumnVector sys_noise_Mu(6);
sys_noise_Mu(1) = 0.0000001;
sys_noise_Mu(2) = 0.0000001;
sys_noise_Mu(3) = 0.0000001;
sys_noise_Mu(4) = 0.0000001;
sys_noise_Mu(5) = 0.0000001;
sys_noise_Mu(6) = 0.0000001;
SymmetricMatrix sys_noise_Cov(6);
sys_noise_Cov = 0.0;
sys_noise_Cov(1, 1) = 0.0000001;
sys_noise_Cov(2, 2) = 0.0000001;
sys_noise_Cov(3, 3) = 0.0000001;
sys_noise_Cov(4, 4) = 0.0000001;
sys_noise_Cov(5, 5) = 0.0000001;
sys_noise_Cov(6, 6) = 0.0000001;
_system_Uncertainty = new Gaussian(sys_noise_Mu, sys_noise_Cov);
_sys_pdf = new NonLinearAnalyticConditionalGaussianMobile(*_system_Uncertainty);
_sys_model = new AnalyticSystemModelGaussianUncertainty(_sys_pdf);
}
void MeasureUncertainty(){
ColumnVector meas_noise_Mu(2);
meas_noise_Mu(1) = 0.001;
meas_noise_Mu(2) = 0.001;
SymmetricMatrix meas_noise_Cov(2);
meas_noise_Cov = 0.0;
meas_noise_Cov(1, 1) = 0.001;
meas_noise_Cov(2, 2) = 0.001;
_measurement_Uncertainty = new Gaussian(meas_noise_Mu, meas_noise_Cov);
// create matrix _meas_model for linear measurement model
double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
Matrix H(2, 6);
H = 0.0;
H(1, 1) = 1;
H(1, 2) = 0;
H(1, 3) = 0;
H(1, 4) = 0;
H(1, 5) = 0;
H(1, 6) = 0;
H(2, 1) = 0;
H(2, 2) = 1;
H(2, 3) = 0;
H(2, 4) = 0;
H(2, 5) = 0;
H(2, 6) = 0;
// create the measurement model
_meas_pdf = new LinearAnalyticConditionalGaussian(H, *_measurement_Uncertainty);
_meas_model = new LinearAnalyticMeasurementModelGaussianUncertainty(_meas_pdf);
}
void PriorInitialize(){
/****************************
* Linear prior DENSITY *
***************************/
// Continuous Gaussian prior (for Kalman filters)
ColumnVector prior_Mu(6);
prior_Mu = 0.0;
SymmetricMatrix prior_Cov(6);
prior_Cov = 0.0;
Gaussian prior_cont(prior_Mu,prior_Cov);
/******************************
* Construction of the Filter *
******************************/
filter = new ExtendedKalmanFilter(&prior_cont);
}
void topic_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {
// RCLCPP_INFO(this->get_logger(), "I heard: %d %d %d", msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z);
static float vx = 0;
static float vy = 0;
static bool catch_once = true;
ColumnVector input(2);
input = 0;
static std::chrono::steady_clock::time_point time_last;
std::chrono::system_clock::time_point now = std::chrono::system_clock::now();
std::time_t now_c = std::chrono::system_clock::to_time_t(now - std::chrono::hours(24));
std::cout << "24 hours ago, the time was "
<< std::put_time(std::localtime(&now_c), "%F %T") << '\n';
std::chrono::steady_clock::time_point time_now = std::chrono::steady_clock::now();
std::cout << "Printing took "
<< std::chrono::duration_cast<std::chrono::microseconds>(time_last - time_now).count()
<< "us.\n";
eskf_imu_filter_.predict(0.02);
eskf_imu_filter_.correctGyr(msg->angular_velocity.x,
msg->angular_velocity.y,
msg->angular_velocity.z);
eskf_imu_filter_.correctAcc(msg->linear_acceleration.x,
msg->linear_acceleration.y,
msg->linear_acceleration.z);
eskf_imu_filter_.reset();
cout << "ax " << msg->angular_velocity.x << " ay " << msg->angular_velocity.y << " az " << msg->angular_velocity.z << endl;
IMU_EKF::Quaternion eskf_quat = eskf_imu_filter_.getAttitude();
float eskf_imu_ax, eskf_imu_ay, eskf_imu_az;
eskf_imu_filter_.getAcceleration(eskf_imu_ax,
eskf_imu_ay,
eskf_imu_az);
Eigen::Quaternion eskf_imu_qv_acc((float)0, eskf_imu_ax, eskf_imu_ay, eskf_imu_az);
Eigen::Quaternion eskf_imu_q(eskf_quat[IMU_EKF::v1],
eskf_quat[IMU_EKF::v2],
eskf_quat[IMU_EKF::v3],
eskf_quat[IMU_EKF::w]);
eskf_imu_qv_acc = (eskf_imu_q.inverse()) * eskf_imu_qv_acc * (eskf_imu_q.inverse()).conjugate();
float vx_temp, vy_temp, v_norm;
vx = msg->linear_acceleration.x * 0.02;
vy = msg->linear_acceleration.y * 0.02;
input(1) = (float)eskf_imu_qv_acc.x();
input(2) = (float)eskf_imu_qv_acc.y();
tf2::Vector3 pos = tflisten->getPos();
ColumnVector meas_state(2);
meas_state(1) = pos[0];
meas_state(2) = pos[1];
tf2::Quaternion q_pose(eskf_imu_q.x(),
eskf_imu_q.y(),
eskf_imu_q.z(),
eskf_imu_q.w());
////
ColumnVector res(2);
// res = _meas_model->Simulate(meas_state);
//
////// tfbro->sendTF(meas_ret, q_pose);
////
cout << "_state: " << _state << endl;
// cout << "meas " << res << endl;
////
filter->Update(_sys_model, input, _meas_model, meas_state);
cout << "measure input state" << meas_state << endl;
// filter->Update(_meas_model, meas_state);
//
Pdf<ColumnVector> *posterior = filter->PostGet();
cout << "input: " << input << endl;
cout << "posterior Mean" << posterior->ExpectedValueGet() << endl;
tf2::Vector3 expected_data;
expected_data[0] = posterior->ExpectedValueGet()(1);
expected_data[1] = posterior->ExpectedValueGet()(2);
expected_data[2] = 0;
// // data send back to tf.
tfbro->sendTF(expected_data, q_pose);
time_last = time_now;
}
just about [x, y, v_x, v_y, a_x, a_y]
something just happen when I raise the rate to 1000hz . the v_y state just suddenly raise to -2.98118e+130 when the input of a_y is still 0.00297971.
is there some thing wrong with the _Mu_new initialize that i found
Pdf<ColumnVector> *posterior = filter->PostGet(); posterior->ExpectedValueGet()
here seem not be zero from the beginning.And found the value fromposterior->ExpectedValueGet()
may come from the class of KalmanFilter from here http://docs.ros.org/en/jade/api/bfl/html/kalmanfilter_8cpp_source.htmlData Showing
here form the data of my terminal output. we can see the input all be zero at the beginning ,but the posterior Mean seem strange
posterior Mean[6](-2.18436e+177,1.32349e-23,1e-07,1e-07,1e-07,1e-07)
and here is the nonlinearanalyticconditionalgaussianmobile.cpp