激光SLAM后端优化——雅克比矩阵推导

Jacobi Matrix

In the EKF system, the maintained state quantities include three categories: IMU state, camera state, and Lidar state, which are expressed as follows:

$X_{imu}=\left[\begin{array}{ccccc}{}^n{q}_b^T& b_g^T& {}^n{v}_b^T& b_a^T& {}^n{p}_b^T\end{array}\right]$

$X_{cam}=\left[\begin{array}{cc}{}^G{q}_{c_0}^T& {}^G{p}_{c_0}^T\end{array}\right]$

$X_{lidar}=\left[\begin{array}{cc}{}^G{q}_L^T& {}^G{p}_L^T\end{array}\right]$

Note: q stands for attitude and p stands for position.Normally,G stands for Earth-Centered,Earth-Fixed Coordinate System，n stands for Navigation System.

We combine the above three state variables into a total state variable,which are expressed as follows:

$X_{old}=\left[\begin{array}{ccc}{X}_{imu}& (X_{cam}{)}_N& (X_{lidar}{)}_M\end{array}\right]$

So far, we have obtained the initial state variables of the entire system. And then,Let’s try to expand our state variables.

If we get a new lidar scan,we need to expand $X$ just like this:

$X_{new}=\left[\begin{array}{cccc}{X}_{imu}& (X_{cam}{)}_N& (X_{lidar}{)}_M& X_{lidar}^{new}\end{array}\right]=\left[\begin{array}{cc}{X}_{old}& X_{lidar}^{new}\end{array}\right]$

For the covariance matrix, it can be expressed in the form of the following figure, in which the green blocks represent non-zero blocks, the blue blocks are all zero matrixes, and the orange blocks represent the content to be obtained when extending the lidar state of the k+1 th frame.

The expanded covariance is:

$P_{new}=\left[\begin{array}{cc}{P}_{old}& P_{O{L}_{k+1}}\\ P_{L_{k+1}O}& P_{L{L}_{l+1}}\end{array}\right]$

📌 Let’s discuss some knowledge about the Jacobian matrix.For simplicity, we use a one-dimensional function as an example.If there is function curve y=f(x,t),and this is a point (x_0,y_0) in the curve i.e. y_0=f(x_0,t_0).

If I want to know the value of a point at $t_1$ on the curve that is very close to the point $(x_0,y_0)$.If I know the X coordinate of this point is $x_1$,so how can I get the Y coordinate of this point if the function at $t_1$ is very similar to that at $t_0$. I can get Y as follows:

$f(x_1,t_1)=y_0+\frac{df(x,t_0)}{dx}{|}_{x=x0}(x_1-x_0)$

For n-dimensional functions we use the Jacobian matrix $J$ to represent the partial derivatives.We can get this:

$f(X_1,t_1)=f(X_0,t_0)+J(X_1-X_0)$

From the law of covariance propagation, we can draw the following conclusions:

$D_{X_1{X}_1}=J{D}_{X_0{X}_0}{J}^T$

$D_{X_0{X}_1}=D_{X_0{X}_0}{J}^T$

$D_{X_1{X}_0}=J{D}_{x_0{x}_0}$

So,We can get:

$$\begin{gathered} P_{new}=\left[\begin{array}{cc}{P}_{old}& P_{O{L}_{k+1}}\\ P_{L_{k+1}O}& P_{L{L}_{l+1}}\end{array}\right] \\ =\left[\begin{array}{cc}{P}_{old}& P_{old}{J}^T\\ J{P}_{old}& J{P}_{old}{J}^T\end{array}\right] \\ =\left[\begin{array}{c}I\\ J\end{array}\right]P_{old}\left[\begin{array}{cc}{I}^T& J^T\end{array}\right] \end{gathered}$$

Obviously,$J$ is the Jacobian matrix of the newly added lidar state variable $X_{lidar}^{new}$ about the previous state variable $X_{old}$. Mathematically, the newly added state variable $X_{lidar}^{new}$is only related to the state variable of imu.
First, construct the position and attitude of the lidar:

$R{(}^G{q}_L)=R{(}^G{q}_n)R{(}^n{q}_b)R_L^b$

${}^G{p}_L=R{(}^G{q}_b)p_L{+}^G{p}_B=R{(}^G{q}_N)R{(}^n{q}_b{)}^b{+}^G{p}_b$

Find partial derivatives by adding perturbation terms,for position:

${}^G{p}_L+{\delta }^G{p}_L=R{(}^G{q}_n)(I-[\delta \theta {]}_{\text{x}})R{(}^n{q}_b{)}^b{p}_L{+}^G{p}_b+{\delta }^G{p}_b$

We can get the following formula：

${\delta }^G{p}_L=-[\delta {\theta }_{\text{x}}]R{(}^G{q}_n)R{(}^n{q}_b{)}^b{p}_L+{\delta }^G{p}_b=R{(}^G{q}_N)[R{(}^n{q}_b{)}^b{p}_L{]}_{\text{x}}\delta \theta +{\delta }^G{p}_b$

Obviously,

$\frac{\partial {\delta }^G{p}_L}{\partial \delta \theta }=R{(}^G{q}_n)[R{(}^n{q}_b{)}^b{p}_L{]}_{\text{x}}$

$\frac{\partial {\delta }^G{p}_L}{{\delta }^G{p}_b}=I$

And for attitude:

$R{(}^G{q}_L)=R{(}^G{q}_n)R{(}^n{q}_b)R_L^b$

Find partial derivatives by adding perturbation terms:

$R{(}^G{q}_L)(I+[{\delta }^G{\theta }_L{]}_{\text{x}})=R{(}^G{q}_n)(I-[{\delta }^n{\theta }_b{]}_{\text{x}})R{(}^n{q}_b)R_L^b$

We can get the following formula：

$R{(}^G{q}_L)[{\delta }^G{\theta }_L{]}_{\text{x}}=-R{(}^G{q}_n)[{\delta }^n\theta {]}_{\text{x}}R{(}^n{q}_b)R_L^b$

$[{\delta }^G{\theta }_L{]}_x=-R_n^L[{\delta }^n\theta {]}_{\text{x}}(R_n^L{)}^T$

$[{\delta }^G{\theta }_L{]}_x=-[R_n^L{\delta }^n{\theta }_b{]}_{\text{x}}$

Obviously,

$\frac{\partial {\delta }^G{\theta }_L}{\partial \delta \theta }=-R_n^L$

$\frac{\partial {\delta }^G{\theta }_L}{{\partial }^G{p}_b}=0$

To sum up, we can obtain the Jacobian matrix：

$J=\left[\begin{array}{ccccccc}-R_n^L& 0& 0& 0& 0& ...& 0\\ R{(}^G{q}_n)[R{(}^n{q}_b{)}^b{p}_L{]}_{\text{x}}& 0& 0& 0& I& ...& 0\end{array}\right]$