深蓝学院高翔《自动驾驶与机器人中的SLAM技术》第二章作业

第二章作业

第三章作业详解
第四章作业详解

第一题

第二题

第三题

• 设置世界坐标系下加速度

  DEFINE_double(acceleration, 9.8, “加速度”);

• 更新自身位置

  Vec3d v_world = pose.so3() * v_body;

• 更新世界坐标系下的位置

  pose.translation() += v_world * dt + a_world * dt * dt * 0.5;

• 将世界系下的加速度转换为本体加速度 TBW = TWB逆

  Vec3d a_body = pose.so3().inverse() * a_world;

• 更新本体系速度

  v_body += a_body * dt;

• 更新旋转

  pose.so3() = pose.so3() * SO3::exp(omega * dt);

• 保存pose，方便绘图

• 绘制轨迹图

第四题

• 高斯牛顿法

高斯牛顿法通过将 J * J(转置) 来作为海森矩阵的近似，避免了对求解增量方程的过程中要求海森矩阵可逆，省略了海森矩阵的计算过程。但可能出现J * J(转置)为奇异矩阵或者病态的情况。

• 列文伯格-马夸尔特方法

在高斯牛顿法的基础上对增量方程进行了优化，添加了一个系数单位矩阵来修正海森矩阵。当系数较小时，海森矩阵占主导，类似高斯牛顿法；系数较大时，系数矩阵占主导，类似于最速下降法。一定程度上避免了矩阵的非奇异与病态的问题。