网站上实用的h5特效,怎么建立企业网站平台,番禺建设银行网站首页,学网站ui设计以下所有内容均为高翔大神所注的《自动驾驶与机器人中的SLAM技术》中的内容
融合导航
1. EKF和优化的关系 2. 组合导航eskf中的预测部分#xff0c;主要是F矩阵的构建
template typename S
bool ESKFS::Predict(const IMU imu) {assert(imu.timestamp…以下所有内容均为高翔大神所注的《自动驾驶与机器人中的SLAM技术》中的内容
融合导航
1. EKF和优化的关系 2. 组合导航eskf中的预测部分主要是F矩阵的构建
template typename S
bool ESKFS::Predict(const IMU imu) {assert(imu.timestamp_ current_time_);double dt imu.timestamp_ - current_time_;if (dt (5 * options_.imu_dt_) || dt 0) {// 时间间隔不对可能是第一个IMU数据没有历史信息LOG(INFO) skip this imu because dt_ dt;current_time_ imu.timestamp_;return false;}// nominal state 递推VecT new_p p_ v_ * dt 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt 0.5 * g_ * dt * dt;VecT new_v v_ R_ * (imu.acce_ - ba_) * dt g_ * dt;SO3 new_R R_ * SO3::exp((imu.gyro_ - bg_) * dt);R_ new_R;v_ new_v;p_ new_p;// 其余状态维度不变// error state 递推// 计算运动过程雅可比矩阵 F见(3.47)// F实际上是稀疏矩阵也可以不用矩阵形式进行相乘而是写成散装形式这里为了教学方便使用矩阵形式Mat18T F Mat18T::Identity(); // 主对角线F.template block3, 3(0, 3) Mat3T::Identity() * dt; // p 对 vF.template block3, 3(3, 6) -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt; // v对thetaF.template block3, 3(3, 12) -R_.matrix() * dt; // v 对 baF.template block3, 3(3, 15) Mat3T::Identity() * dt; // v 对 gF.template block3, 3(6, 6) SO3::exp(-(imu.gyro_ - bg_) * dt).matrix(); // theta 对 thetaF.template block3, 3(6, 9) -Mat3T::Identity() * dt; // theta 对 bg// mean and cov predictiondx_ F * dx_; // 这行其实没必要算dx_在重置之后应该为零因此这步可以跳过但F需要参与Cov部分计算所以保留cov_ F * cov_.eval() * F.transpose() Q_;current_time_ imu.timestamp_;return true;
}
3. 以下是速度量测主要是H矩阵的构建
template typename S
bool ESKFS::ObserveWheelSpeed(const Odom odom) {assert(odom.timestamp_ current_time_);// odom 修正以及雅可比// 使用三维的轮速观测H为3x18大部分为零Eigen::MatrixS, 3, 18 H Eigen::MatrixS, 3, 18::Zero();H.template block3, 3(0, 3) Mat3T::Identity();// 卡尔曼增益Eigen::MatrixS, 18, 3 K cov_ * H.transpose() * (H * cov_ * H.transpose() odom_noise_).inverse();// velocity obsdouble velo_l options_.wheel_radius_ * odom.left_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;double velo_r options_.wheel_radius_ * odom.right_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;double average_vel 0.5 * (velo_l velo_r);VecT vel_odom(average_vel, 0.0, 0.0);VecT vel_world R_ * vel_odom;dx_ K * (vel_world - v_);//v_是状态递推后的速度// update covcov_ (Mat18T::Identity() - K * H) * cov_;UpdateAndReset();return true;
}
4. 以下是GPS的量测主要任然是H矩阵的构建
template typename S
bool ESKFS::ObserveGps(const GNSS gnss) {/// GNSS 观测的修正assert(gnss.unix_time_ current_time_);if (first_gnss_) {R_ gnss.utm_pose_.so3();p_ gnss.utm_pose_.translation();first_gnss_ false;current_time_ gnss.unix_time_;return true;}assert(gnss.heading_valid_);ObserveSE3(gnss.utm_pose_, options_.gnss_pos_noise_, options_.gnss_ang_noise_);current_time_ gnss.unix_time_;return true;
}template typename S
bool ESKFS::ObserveSE3(const SE3 pose, double trans_noise, double ang_noise) {/// 既有旋转也有平移/// 观测状态变量中的p, RH为6x18其余为零Eigen::MatrixS, 6, 18 H Eigen::MatrixS, 6, 18::Zero();H.template block3, 3(0, 0) Mat3T::Identity(); // P部分H.template block3, 3(3, 6) Mat3T::Identity(); // R部分3.66)// 卡尔曼增益和更新过程Vec6d noise_vec;noise_vec trans_noise, trans_noise, trans_noise, ang_noise, ang_noise, ang_noise;Mat6d V noise_vec.asDiagonal();Eigen::MatrixS, 18, 6 K cov_ * H.transpose() * (H * cov_ * H.transpose() V).inverse();// 更新x和covVec6d innov Vec6d::Zero();innov.template head3() (pose.translation() - p_); // 平移部分innov.template tail3() (R_.inverse() * pose.so3()).log(); // 旋转部分(3.67)dx_ K * innov;cov_ (Mat18T::Identity() - K * H) * cov_;UpdateAndReset();return true;
}
