第六章作业

最新推荐文章于 2024-11-06 09:27:45 发布

Teamo1996

最新推荐文章于 2024-11-06 09:27:45 发布

阅读量116

点赞数

文章标签：自动驾驶

本文链接：https://blog.csdn.net/weixin_39061796/article/details/119788014

版权

深蓝学院-机器人学中的状态估计专栏收录该内容

6 篇文章 2 订阅

订阅专栏

第六章作业

1

设
$\begin{bmatrix} c_1 & c_2 & c_3 \end{bmatrix} \\ u = \begin{bmatrix} u_1 \\ u_2 \\ u_3 \end{bmatrix}$
则
$C^T (Cu)^{\wedge}C = C^T (c_1u_1 + c_2u_2 + c_3 u_3)^{\wedge} C \\ = C^Tv_u c_1^{\wedge} C + C^Tv_u c_2^{\wedge} C + C^Tv_u c_3^{\wedge} C \\$
其中
$C^Tu_1c_1^{\wedge}C = u_1 \begin{bmatrix} c_1^T c_1^{\wedge}c_1 & c_1^T c_1^{\wedge}c_2 & c_1^T c_1^{\wedge}c_3 \\ c_2^T c_1^{\wedge}c_1 & c_2^T c_1^{\wedge}c_2 & c_2^T c_1^{\wedge}c_3 \\ c_3^T c_1^{\wedge}c_1 & c_3^T c_1^{\wedge}c_2 & c_3^T c_1^{\wedge}c_3\end{bmatrix} \\ = u_1 \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{bmatrix}$

$C^Tu_2c_2^{\wedge}C = u_2 \begin{bmatrix} c_1^T c_2^{\wedge}c_1 & c_1^T c_2^{\wedge}c_2 & c_1^T c_2^{\wedge}c_3 \\ c_2^T c_2^{\wedge}c_1 & c_2^T c_2^{\wedge}c_2 & c_2^T c_2^{\wedge}c_3 \\ c_3^T c_2^{\wedge}c_1 & c_3^T c_2^{\wedge}c_2 & c_3^T c_1^{\wedge}c_3\end{bmatrix} \\ = u_2 \begin{bmatrix} 0 & 0 & 1 \\ 0 & 0 & 0 \\ -1 & 0 & 0 \end{bmatrix}$

$C^Tu_3c_3^{\wedge}C = u_3 \begin{bmatrix} c_1^T c_3^{\wedge}c_1 & c_1^T c_3^{\wedge}c_2 & c_1^T c_3^{\wedge}c_3 \\ c_2^T c_3^{\wedge}c_1 & c_2^T c_3^{\wedge}c_2 & c_2^T c_3^{\wedge}c_3 \\ c_3^T c_3^{\wedge}c_1 & c_3^T c_3^{\wedge}c_2 & c_3^T c_3^{\wedge}c_3\end{bmatrix} \\ = u_3 \begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix}$

加起来可得
$C^T (Cu)^{\wedge}C = u^{\wedge}$
左右同乘 $C$ 和 $C^T$ ，可证
$(Cu)^{\wedge} = Cu^{\wedge}C^T$

2.

根据表7-2，有如下性质
$(Cu)^{\wedge} = u^{\wedge}(tr(C)1 - C) - C^Tu^{\wedge}$

$2cos\phi + 1$

代入得
$(Cu)^{\wedge} = u^{\wedge}(tr(C)1 - C) - C^Tu^{\wedge}\\ = u^{\wedge}((2cos\phi+1)I - C)-C^Tu^{\wedge} \\ = 2cos\phi u^{\wedge} - u^{\wedge}C - C^Tu^{\wedge}$
证闭

3.

根据第一题的证明可得
$exp((Cu)^{\wedge}) = exp(Cu^{\wedge}C^T) \\ = \sum_{n=0}^{\infty} \frac{1}{n!}{(Cu^{\wedge}C^T)^n} \\ = Cu^{\wedge}C^T + \frac{1}{2}Cu^{\wedge}C^TCu^{\wedge}C^T + ... + \frac{1}{n!}C(u^{\wedge})^nC^T \\ = C\sum_{n=0}^{\infty}{\frac{1}{n!} (u^{\wedge})^n}C^T \\ = Cecp(u^{\wedge})C^T$

4.

根据表7-3可得
$(\mathcal{T}x)^{\wedge} = (\begin{bmatrix} C & r^{\wedge}C \\ 0 & C \end{bmatrix}\begin{bmatrix} u \\ v \end{bmatrix})^{\wedge} \\ = \begin{bmatrix} Cu + r^{\wedge}Cv \\ Cv \end{bmatrix}^{\wedge} \\ = \begin{bmatrix} (Cv)^{\wedge} & Cu + r^{\wedge}Cv \\ 0 & 0 \end{bmatrix} \\ = \begin{bmatrix} C & r \\ 0 & C \end{bmatrix} \begin{bmatrix} v^{\wedge}C^T & u + C^{-1}r^{\wedge}Cv \\ 0 & 0 \end{bmatrix} \\ = \begin{bmatrix} C & r \\ 0 & C \end{bmatrix} \begin{bmatrix} v^{\wedge}C^T & u + (C^Tr)^{\wedge}v \\ 0 & 0 \end{bmatrix} \\ = \begin{bmatrix} C & r \\ 0 & C \end{bmatrix} \begin{bmatrix} v^{\wedge} & u \\ 0 & 0 \end{bmatrix} \begin{bmatrix} C^T & - C^T r \\ 0 & 1 \end{bmatrix} \\ = T x^{\wedge}T^{-1}$
证闭

5.

这个证明与第三题类似，此处不重复进行指数的展开
$exp((\mathcal{T}x)^{\wedge}) = exp(Tx^{\wedge}T^{-1}) \\ = \sum_{n=0}^{\infty}{\frac{1}{n!}(Tx^{\wedge}T^{-1})^n} \\ = \sum_{n=0}^{\infty}{\frac{1}{n!}(Tx^{\wedge})^nT^{-1}} \\ = T \sum_{n=0}^{\infty}{\frac{1}{n!}(x^{\wedge})^n} T^{-1}\\ = T exp(x^{\wedge})T^{-1}$

7.

$x^{\wedge}p = \begin{bmatrix} u \\ v \end{bmatrix}^{\wedge} \begin{bmatrix} \epsilon \\ \eta \end{bmatrix} \\ = \begin{bmatrix} v^{\wedge} & u \\ 0 & 0 \end{bmatrix}\begin{bmatrix} \epsilon \\ \eta \end{bmatrix} \\ = \begin{bmatrix} v^{\wedge}\epsilon + u\eta \\ 0 \end{bmatrix} \\ = \begin{bmatrix} -\epsilon^{\wedge}v + u\eta \\ 0 \end{bmatrix} \\ = \begin{bmatrix} \eta I & -\epsilon^{\wedge} \\ 0 & 0 \end{bmatrix} \begin{bmatrix} u \\ v\end{bmatrix} \\ = p^{\odot}x$

证闭

8.

$p^Tx^{\wedge} = \begin{bmatrix} \epsilon^T & \eta^T \end{bmatrix} \begin{bmatrix} v^{\wedge} & u \\ 0 & 0 \end{bmatrix} \\ = \begin{bmatrix} \epsilon^T v ^{\wedge} & \epsilon^T u \end{bmatrix} \\ = \begin{bmatrix} (-v^{\wedge}\epsilon)^T & \epsilon^T u \end{bmatrix} \\ = \begin{bmatrix} (\epsilon^{\wedge}v)^T & \epsilon^T u \end{bmatrix} \\ = \begin{bmatrix} -v^T \epsilon^{\wedge} & u^T \epsilon \end{bmatrix} \\ = \begin{bmatrix} u^T & v^T \end{bmatrix} \begin{bmatrix} 0 & \epsilon \\ - \epsilon^{\wedge} & 0 \\ \end{bmatrix} \\ = x^T p^{\circledcirc}$

证闭

11.

$(Tp)^{\odot} = (\begin{bmatrix} C & r \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \epsilon \\ \eta \end{bmatrix})^{\odot} \\ = \begin{bmatrix} C\epsilon + \eta r \\ \eta\end{bmatrix}^{\odot} \\ = \begin{bmatrix} \eta & -(C \epsilon + \eta r)^{\wedge} \\ 0 & 0 \end{bmatrix}$

$p^{\odot}\mathcal{T}^{-1} = \begin{bmatrix} C & r \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \eta & -\epsilon^{\wedge} \\ 0 & 0\end{bmatrix} \begin{bmatrix}C^T & -C^Tr^{\wedge} \\ 0 & C^T \end{bmatrix} \\ = \begin{bmatrix} \eta C & -C \epsilon^{\wedge} \\ 0 & 0 \end{bmatrix} \begin{bmatrix}C^T & -C^Tr^{\wedge} \\ 0 & C^T \end{bmatrix} \\ = \begin{bmatrix} \eta & -\eta r^{\wedge} - C \epsilon^{\wedge}C^T \\ 0 & 0 \end{bmatrix} \\ = \begin{bmatrix} \eta & -\eta r^{\wedge} - (C \epsilon)^{\wedge} \\ 0 & 0 \end{bmatrix}\\ =\begin{bmatrix} \eta & -(C \epsilon + \eta r)^{\wedge} \\ 0 & 0 \end{bmatrix}$

等式左右两边相等，证闭

12.

根据11题的结论
${(Tp)^{\odot}}^T(Tp)^{\odot} \\ = (Tp^{\odot}\mathcal{T}^{-1})^T(Tp^{\odot}\mathcal{T}^{-1}) \\ = {\mathcal{T}^{-1}}^T {p^{\odot}}^TT^{T}Tp^{\odot}\mathcal{T}^{-1}\\ = {\mathcal{T}^{-1}}^T {p^{\odot}}^Tp^{\odot}\mathcal{T}^{-1}$
证闭