做网站浏览器必须用xp系统吗,各大网站域名大全,天津建设工程信息网怎么注册,怎么区别网站开发语言ESKF 总结
prediction
更新误差先验 F F F通过3.42来算 得到 这里有点绕的一点是: 误差状态的 F F F牵涉到名义状态, 而名义状态又需要在时间上推进更新
其中, F中的名义状态的推进通过公式3.41得到,
(名义状态不考虑误差, 这一点从3.41d, 3.41e可以看出, 误差状态只考虑…ESKF 总结
prediction
更新误差先验 F F F通过3.42来算 得到 这里有点绕的一点是: 误差状态的 F F F牵涉到名义状态, 而名义状态又需要在时间上推进更新
其中, F中的名义状态的推进通过公式3.41得到,
(名义状态不考虑误差, 这一点从3.41d, 3.41e可以看出, 误差状态只考虑误差, 真实状态为名义误差)
代码的实现
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;
}Q Q Q的算法 注意, 在离散情况下 η v η a Δ t η θ η g Δ t η g η b g Δ t η a η b a Δ t \begin{aligned} \eta_v \eta_a \Delta t\\ \eta_ \theta \eta_g \Delta t \\ \eta_g \eta_{bg} \Delta t\\ \eta_ a \eta_{ba} \Delta t \\ \end{aligned} ηvηaΔtηθηgΔtηgηbgΔtηaηbaΔt 可以根据3.40将3.42进行一阶泰勒展开
代码中的实现 double ev options.acce_var_;double et options.gyro_var_;double eg options.bias_gyro_var_;double ea options.bias_acce_var_;double ev2 ev; // * ev; // tj : 为什么没平方? Q里面应该是方差double et2 et; // * et;double eg2 eg; // * eg;double ea2 ea; // * ea;// 设置过程噪声Q_.diagonal() 0, 0, 0, ev2, ev2, ev2, et2, et2, et2, eg2, eg2, eg2, ea2, ea2, ea2, 0, 0, 0;其中options的定义 struct Options {Options() default;/// IMU 测量与零偏参数double imu_dt_ 0.01; // IMU测量间隔// NOTE IMU噪声项都为离散时间不需要再乘dt可以由初始化器指定IMU噪声double gyro_var_ 1e-5; // 陀螺测量标准差double acce_var_ 1e-2; // 加计测量标准差double bias_gyro_var_ 1e-6; // 陀螺零偏游走标准差double bias_acce_var_ 1e-4; // 加计零偏游走标准差correction
更新误差后验 H H H是观测值对误差状态变量的jacob
先看GNSS, 它观测值有位移和旋转, 根据3.66和3.70可得 H H H,
GNSS对旋转的观测是总体的旋转, 然而名义上的 R R R是知道的, 所以我们可以将观测值变以成对于旋转误差状态的直接观测, 这样一来, 它对自己求导就为 I I I
同理, GNSS对位移的观测是总体的位移, 然后名义上的 p p p是知道, 所以我们将对总体位移的观测转到对于误差位移的直接观测 Eigen::MatrixS, 6, 18 H Eigen::MatrixS, 6, 18::Zero();H.template block3, 3(0, 0) Mat3T::Identity(); // P部分 (3.70)H.template block3, 3(3, 6) Mat3T::Identity(); // R部分3.66)然后求 V V V // 卡尔曼增益和更新过程Vec6d noise_vec;noise_vec trans_noise, trans_noise, trans_noise, ang_noise, ang_noise, ang_noise;Mat6d V noise_vec.asDiagonal();其中trans_noise, ang_noise在option中定义 /// RTK 观测参数double gnss_pos_noise_ 0.1; // GNSS位置噪声double gnss_height_noise_ 0.1; // GNSS高度噪声double gnss_ang_noise_ 1.0 * math::kDEG2RAD; // GNSS旋转噪声然后更新后验3.51b, 3.51d // 更新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_;有了误差状态, 就可以更新名义状态3.51c /// 更新名义状态变量重置error statevoid UpdateAndReset() {p_ dx_.template block3, 1(0, 0);v_ dx_.template block3, 1(3, 0);R_ R_ * SO3::exp(dx_.template block3, 1(6, 0));if (options_.update_bias_gyro_) {bg_ dx_.template block3, 1(9, 0);}if (options_.update_bias_acce_) {ba_ dx_.template block3, 1(12, 0);}g_ dx_.template block3, 1(15, 0);ProjectCov();dx_.setZero();}注意 这里有一步需要投影, 参考3.63 /// 对P阵进行投影参考式(3.63)
void ProjectCov() {Mat18T J Mat18T::Identity();J.template block3, 3(6, 6) Mat3T::Identity() - 0.5 * SO3::hat(dx_.template block3, 1(6, 0));cov_ J * cov_ * J.transpose();
}再看odom, 它观测值只有速度
首先求H, 注意这里观测值为本地速度, 但是名义变量 R R R这时是知道的, 所以可以把观测值从本地速度转化到世界速度, 然后世界速度对世界速度求导就为 I I I // odom 修正以及雅可比
// 使用三维的轮速观测H为3x18大部分为零
Eigen::MatrixS, 3, 18 H Eigen::MatrixS, 3, 18::Zero();
H.template block3, 3(0, 3) Mat3T::Identity();然后算V
// 设置里程计噪声
double o2 options_.odom_var_ * options_.odom_var_;
odom_noise_.diagonal() o2, o2, o2;option里面的定义
/// 里程计参数
double odom_var_ 0.5;
double odom_span_ 0.1; // 里程计测量间隔
double wheel_radius_ 0.155; // 轮子半径
double circle_pulse_ 1024.0; // 编码器每圈脉冲数然后更新后验
本地速度观测值的算法参见 3.76
// velocity obs
double 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_);// update cov
cov_ (Mat18T::Identity() - K * H) * cov_;
UpdateAndReset();
