Some question about the Jacobian

[q87956256]

Thank you for your wonderful work!
I have two questions about the code:

1.In formula 8, the results from the code corresponding to the Jacobian derivation of the error with respect to the error state variables are different from those I derived.
In your code, the Jacobian of the error with respect to the error state variables is:

However, my derivation process and results are shown in the figure below:

That is different with your code.
Could you kindly share your derivation process?

2.In formula 9, I have the same question about the Jacobian:
In your code, the Jacobian of the error with respect to the error state variables is:

However, my derivation process and results are shown in the figure below:

That is different with your code.
Could you kindly share your derivation process?

[Taeyoung96]

@q87956256
Thanks for interesting our work!

I went back through the derivation process and it seems that I made a terrible mistake in my approch 😢

If I rearrange it, it looks like this,

The state vector $(\mathbf{x}_k)$ is defined as:

$$\mathbf{x}_k = [\mathbf{R}_{L_k}, \mathbf{p}_{L_k}, \boldsymbol{\omega}_{L_k}, \mathbf{v}_{L_k}]$$

where:

• $(\mathbf{R}_{L_k} \in SO(3))$: Rotation matrix
• $(\mathbf{p}_{L_k} \in \mathbb{R}^3)$: Position
• $(\boldsymbol{\omega}_{L_k} \in \mathbb{R}^3)$: Angular velocity
• $(\mathbf{v}_{L_k} \in \mathbb{R}^3)$: Linear velocity

The residual is defined as:

$$\mathbf{z}_{\pi} = \begin{pmatrix} \left[ (\mathbf{R}^G_L)^T \mathbf{R}_{L_k} \cdot \mathbf{n}_{L_k} \right]_{1,2} \\ \\\ \mathbf{e}_3^{\mathbf{T}} \cdot (\mathbf{R}^G_L)^T \mathbf{p}_{L_k} \end{pmatrix}$$

The goal is to compute the Jacobian matrix:

$$\mathbf{H}_{\pi} = \left.\frac{\partial \mathbf{h}_{\pi}(\hat{\mathbf{x}} \boxplus \tilde{\mathbf{x}}) \boxminus \mathbf{z}_{\pi}}{\partial \tilde{\mathbf{x}}}\right|_{\tilde{\mathbf{x}}=\mathbf{0}}$$

---

The first component of the residual $(\mathbf{z}_{\pi,1})$ is:

$$\mathbf{z}_{\pi,1} = \left[ (\mathbf{R}^G_L)^T \mathbf{R}_{L_k} \mathbf{n}_{L_k} \right]_{1,2}.$$

Under rotation perturbation,

$$(\mathbf{R}^G_L)^T \mathbf{R}_{L_k} \rightarrow (\mathbf{R}^G_L)^T \mathbf{R}_{L_k} (\mathbf{I} + [\tilde{\boldsymbol{\theta}}]_{\times}).$$

Expanding this to first order,

$$(\mathbf{R}^G_L)^T \mathbf{R}_{L_k} (\mathbf{I} + [\tilde{\boldsymbol{\theta}}]_{\times}) \mathbf{n}_{L_k} \approx (\mathbf{R}^G_L)^T \mathbf{R}_{L_k} \mathbf{n}_{L_k} + (\mathbf{R}^G_L)^T \mathbf{R}_{L_k} [\tilde{\boldsymbol{\theta}}]_{\times} \mathbf{n}_{L_k}.$$

The skew-symmetric matrix satisfies the property:

$$[\tilde{\boldsymbol{\theta}}]_{\times} \mathbf{v} = - \mathbf{v} \times \tilde{\boldsymbol{\theta}} \quad \text{for any vector } \mathbf{v}.$$

Substituting this into the previous expression:

$$(\mathbf{R}^G_L)^T \mathbf{R}_{L_k} [\tilde{\boldsymbol{\theta}}]_{\times} \mathbf{n}_{L_k} = - (\mathbf{R}^G_L)^T \mathbf{R}_{L_k} (\mathbf{n}_{L_k} \times \tilde{\boldsymbol{\theta}}).$$

Using the property of the cross product:

$$\mathbf{n}_{L_k} \times \tilde{\boldsymbol{\theta}} = [\mathbf{n}_{L_k}]_{\times} \tilde{\boldsymbol{\theta}}.$$

Thus:

$$\frac{\partial \mathbf{z}_{\pi,1}}{\partial \tilde{\boldsymbol{\theta}}} = (\mathbf{R}^G_L)^T \mathbf{R}_{L_k} [\tilde{\boldsymbol{\theta}}]_{\times} \mathbf{n}_{L_k} = - (\mathbf{R}^G_L)^T \mathbf{R}_{L_k} [\mathbf{n}_{L_k}]_{\times} \tilde{\boldsymbol{\theta}}.$$

The second residual component is:

$$\mathbf{z}_{\pi,2} = \mathbf{e}_3^{\mathbf{T}} \cdot (\mathbf{R}^G_L)^T \mathbf{p}_{L_k}.$$ $$\mathbf{z}_{\pi,2} \rightarrow \mathbf{e}_3^{\mathbf{T}} \cdot (\mathbf{R}^G_L)^T (\mathbf{p}_{L_k} + \tilde{\mathbf{p}}).$$

The perturbation of $(\mathbf{z}_{\pi,2})$ is:

$$\Delta \mathbf{z}_{\pi,2} = \mathbf{e}_3^{\mathbf{T}} \cdot (\mathbf{R}^G_L)^T \tilde{\mathbf{p}}.$$

The Jacobian of $(\mathbf{z}_{\pi,2})$ with respect to $(\tilde{\mathbf{p}})$ is therefore:

$$\frac{\partial \mathbf{z}_{\pi,2}}{\partial \tilde{\mathbf{p}}} = \mathbf{e}_3^{\mathbf{T}} \cdot (\mathbf{R}^G_L)^T.$$

Final Jacobian Matrix $(\mathbf{H}_{\pi})$
Combining all terms:

$$\mathbf{H}_{\pi} = \begin{bmatrix} \frac{\partial \mathbf{z}_{\pi,1}}{\partial \tilde{\boldsymbol{\theta}}} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \\\ \mathbf{0}& \frac{\partial \mathbf{z}_{\pi,2}}{\partial \tilde{\mathbf{p}}} & \mathbf{0} & \mathbf{0} \end{bmatrix}.$$ $$\frac{\partial \mathbf{z}_{\pi,1}}{\partial \tilde{\boldsymbol{\theta}}} = - (\mathbf{R}^G_L)^T \mathbf{R}_{L_k} [\mathbf{n}_{L_k}]_{\times} \tilde{\boldsymbol{\theta}}$$ $$\frac{\partial \mathbf{z}_{\pi,2}}{\partial \tilde{\mathbf{p}}} = \mathbf{e}_3^{\mathbf{T}} \cdot (\mathbf{R}^G_L)^T$$

I apologize for the inconvenience and will update the code to match the above formula.

[q87956256]

Thanks for your reply!
Your Jacobian derivation method and results align with mine, which is reassuring.
The last thing I'd like to confirm is:
In your derivation:

Is in your derivation equivalent to in Formula (10) from your paper?

Looking forward to your clarification!
Best regards,