深蓝学院-多传感器融合定位-第5章作业

Github: https://github.com/WeihengXia0123/LiDar-SLAM

第一题：不基于转台的加速度计标定：Jacobian推导

首先，我们计算出向量$\text{a}$的表达：
$$\begin{array}{rl}\text{a}& =(I-S_a)K_a^{\prime }A-b_a\\ & =\left[\begin{array}{ccc}1& 0& 0\\ -S_{ayx}& 1& 0\\ -S_{azx}& -S_{azy}& 1\end{array}\right]\left[\begin{array}{ccc}{K}_{ax}^{-1}& 0& 0\\ 0& K_{ay}^{-1}& 0\\ 0& 0& K_{az}^{-1}\end{array}\right]\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]-\left[\begin{array}{c}{b}_{ax}\\ b_{ay}\\ b_{az}\end{array}\right]\\ & \\ & =...（老衲掐指一算）\\ & \\ & =\left[\begin{array}{c}{K}_{ax}^{-1}{A}_x-b_{ax}\\ -S_{ayx}{K}_{ax}^{-1}{A}_x+K_{ay}^{-1}{A}_y-b_{ay}\\ -S_{ayx}{K}_{ax}^{-1}{A}_x-S_{azy}{K}_{ay}^{-1}{A}_y+K_{az}^{-1}{A}_z-b_{az}\end{array}\right]\end{array}$$

其次，基于上面的结果，咱们就可以计算残差Residual了：
$$\begin{array}{rl}f({\theta }^{acc})& =||g|{|}^2-||a|{|}^2\\ & =g_0^2-a^{T}a\\ & =g_0^2-(K_{ax}^{-1}{A}_x-b_{ax}{)}^2-(-S_{ayx}{K}_{ax}^{-1}{A}_x+K_{ay}^{-1}{A}_y-b_{ay}{)}^2\\ & -(-S_{azx}{K}_{ax}^{-1}{A}_x-S_{azy}{K}_{ay}^{-1}{A}_y+K_{az}^{-1}{A}_z-b_{az}{)}^2\end{array}$$
好了，有了$f(\cdot )$的完整表达形式，咱们就可以计算Jacobian了：

这里因为Residual只有一个$f$，所以$J({\theta }^{acc})$是一个1x9的向量。
通过复合函数的链式求导法则和上面求得的$f$，可以得到：
J 1 = ∂ f ∂ S a y x = − 2 ( − S a y x K a x − 1 A x + K a y − 1 A y − b a y ) ( − K a x − 1 A x ) J 2 = ∂ f ∂ S a z x = − 2 ( − S a z x K a x − 1 A x − S a z y K a y − 1 A y + K a z − 1 A z − b a z ) ( − K a x − 1 A x ) J 3 = ∂ f ∂ S a z y = − 2 ( − S a z x K a x − 1 A x − S a z y K a y − 1