深蓝学院-多传感器集成定位-第五章操作
- 第一个问题基于转台的加速度计校准:Jacobian推导
- 第二题:完成校准(自动求导)
- 第三题:完成校准(解析求导)
-
- 1. 标量对向量求导法
- 2. 向量对向量求导法
: https://github.com/WeihengXia0123/LiDar-SLAM
第一个问题基于转台的加速度计校准:Jacobian推导
首先，我们计算出向量 a \textbf{a} a的表达： a = ( I ? S a ) K a ′ A ? b a = [ 1 0 0 ? S a y x 1 0 ? S a z x ? S a z y 1 ] [ K a x ? 1 0 0 0 K a y ? 1 0 0 0 K a z ? 1 ] [ A x A y A z ] ? [ b a x b a y b a z ] = . . . （ 老 衲 掐 指 一 算 ） = [ K a x ? 1 A x ? b a x ? S a y x K a x ? 1 A x K a y ? 1 A y ? b a y ? S a y 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 ] \begin{aligned} \textbf{a} &= (I-S_a)K'_aA-b_a \\ &= \left[ \begin{matrix} 1 & 0 & 0 \\ -S_{ayx} & 1 & 0 \\ -S_{azx} & -S_{azy} & 1 \end{matrix} \right] \left[ \begin{matrix} K_{ax}^{-1} & 0 & 0 \\ 0 & K_{ay}^{-1} & 0 \\ 0 & 0 & K_{az}^{-1} \end{matrix} \right] \left[ \begin{matrix} A_x\\ A_y\\ A_z \end{matrix} \right] - \left[ \begin{matrix} b_{ax}\\ b_{ay}\\ b_{az} \end{matrix} \right] \\\\ &= ...(老娜掐指一算)\\ \\ &= \left[ \begin{matrix} 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{matrix} \right] \end{aligned} a=(I?Sa)K
其次，基于上面的结果，咱们就可以计算残差Residual了： f ( θ a c c ) = ∣ ∣ g ∣ ∣ 2 − ∣ ∣ a ∣ ∣ 2 = g 0 2 − a T a = g 0 2 − ( K a x − 1 A x − b a x ) 2 − ( − S a y x K a x − 1 A x + K a y − 1 A y − b a y ) 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 ) 2 \begin{aligned} f(\theta^{acc}) &= ||g||^2-||a||^2\\ &= g_{0}^{2} - a^Ta\\ &= 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{aligned} f(θacc)=∣∣g∣∣2−∣∣a∣∣2=g02−aTa=g02−(Kax−1Ax−bax)2−(−SayxKax−1Ax+Kay−1Ay−bay)2 −(−SazxKax−1Ax− 标签： qr30传感器