第二章作业
第一题
第二题
第三题
-
设置世界坐标系下加速度
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(转置)为奇异矩阵或者病态的情况。
- 列文伯格-马夸尔特方法
在高斯牛顿法的基础上对增量方程进行了优化,添加了一个系数单位矩阵来修正海森矩阵。当系数较小时,海森矩阵占主导,类似高斯牛顿法;系数较大时,系数矩阵占主导,类似于最速下降法。一定程度上避免了矩阵的非奇异与病态的问题。