手写VIO课后作业（三）

课后作业

第一题

第一问：绘制阻尼因子迭代变化曲线图

看似简单，但是有细节之处。不仅需要画出代码中打印的正确的lambda，还需要将IsGoodSteplnLm()函数返回flase失败的lambda也要画出来。

第二问：将指数函数换成二次函数，实现曲线函数参数估计

需要修改的核心代码：
1.Main函数中观测方程修改为：

// 观测 y
double y = a*x*x + b*x + c + n;

2.修改残差计算函数为：

// 计算曲线模型误差
virtual void ComputeResidual() override
{
    Vec3 abc = verticies_[0]->Parameters();  // 估计的参数
    residual_(0) = abc(0)*x_*x_ + abc(1)*x_ + abc(2) - y_; //构建残差
}

3.修改雅克比计算函数为：

// 计算残差对变量的雅克比
virtual void ComputeJacobians() override
{
    Vec3 abc = verticies_[0]->Parameters();
    double exp_y = std::exp( abc(0)*x_*x_ + abc(1)*x_ + abc(2) );

    Eigen::Matrix<double, 1, 3> jaco_abc;  // 误差为1维，状态量 3 个，所以是 1x3 的雅克比矩阵
    jaco_abc << x_ * x_, x_ , 1;
    jacobians_[0] = jaco_abc;
}

注意：
本题结果真实值为：a=1，b=2，c=1；若采用原始参数，拟合结果较差，可通过以下操作进行改进：
1.增加数据点数，如N=1000（原始N=100）
2.增大步长以增大数据范围，如x=i/10（原始x=i/100）
3.减小噪声均方差，如w_sigma=0.01（原始w_sigma=0.1）

第三问：实现其他更新阻尼因子策略：

（1）策略一



（2）策略二



第一种策略会比第三种策略速度稍快一点，但是精度差不多。第二种策略相比于第一种和第三种策略，结果精度虽然差不多，但迭代次数会多很多。

第二题

第一问 f15推导

两种方法均可：

第二问：g12推导

第三题