《机器人学中的状态估计》第三讲（第四章）课后习题作业

最新推荐文章于 2022-05-18 11:36:52 发布

走走走走走走你

最新推荐文章于 2022-05-18 11:36:52 发布

阅读量1.3k

点赞数 3

分类专栏：机器人学中的状态估计

本文链接：https://blog.csdn.net/qq_37372155/article/details/108318771

版权

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

10 篇文章 17 订阅

订阅专栏

1、证明教材式（4.31）

2、证明教材式（4.38）

3、考虑如下离散时间系统

1、证明教材式（4.31）

证明：

（放弃）

2、证明教材式（4.38）

（是验证不是推导，我觉得我又行了）

答：（1）对于式

等式右边 =

$\sum_{i=0}^{2L}\alpha_{i}x_{i}$

$\\=\alpha_{0}x_{0}+\sum_{i=0}^{L}\alpha_{i}x_{i}+\sum_{i=L+1}^{2L}\alpha_{i}x_{i}\\ =\alpha_{0}x_{0}+\frac{1}{2}\frac{1}{L+k}\left ( \sum_{i=0}^{L}x_{i}+\sum_{i=L+1}^{2L}x_{i} \right )\\ =\alpha_{0}x_{0}+\frac{1}{2}\frac{1}{L+k}\left ( x_{1}+\cdots +x_{L}+x_{L+1}+\cdots +x_{2L}\right)$

$\\ =\alpha_{0}x_{0}+\frac{1}{2}\frac{1}{L+k}\left (\mu _{x}+\sqrt{L+k}col_{1}L+\cdots+\mu _{x}+\sqrt{L+k}col_{L}L+\mu _{x}-\sqrt{L+k}col_{L+1}L+\cdots+\mu _{x}-\sqrt{L+k}col_{2L}L \right)\\$

$\\=\alpha_{0}x_{0}+\frac{1}{2}\frac{1}{L+k}\left (2L\mu _{x}\right)\\ =\alpha_{0}x_{0}+\frac{L\mu _{x}}{L+k}\\ =\frac{k\mu _{x}}{L+k}+\frac{L\mu _{x}}{L+k}\\ =\mu _{x}\left( \frac{k}{L+k}+\frac{L}{L+k}\right)\\ =\mu _{x}$

（2）对于式

等式右边 =

$\sum_{i=0}^{2L}\alpha _{i}\left (\left ( x_{i}-\mu _{x} \right )\left ( x_{i}-\mu _{x} \right ) ^{T} \right )$

$\\=\alpha _{0}\left ( x_{0}-\mu _{x} \right )\left ( x_{0}-\mu _{x} \right ) ^{T}+\sum_{i=1}^{L}\alpha _{i}\left (\left ( x_{i}-\mu _{x} \right )\left ( x_{i}-\mu _{x} \right ) ^{T} \right )+\sum_{i=L+1}^{2L}\alpha _{i}\left (\left ( x_{i}-\mu _{x} \right )\left ( x_{i}-\mu _{x} \right ) ^{T} \right )$

$\\ =0+\frac{1}{2}\frac{1}{L+k}\left ( \left ( \sqrt{L+k} col_{1}L\right )\left ( \sqrt{L+k} col_{1}L\right )^{T} +\cdots + \left ( \sqrt{L+k} col_{L}L\right )\left ( \sqrt{L+k} col_{L}L\right )^{T}+ \left (- \sqrt{L+k} col_{L+1}L\right )\left ( -\sqrt{L+k} col_{L+1}L\right )^{T}+\cdots + \left (- \sqrt{L+k} col_{2L}L\right )\left ( -\sqrt{L+k} col_{2L}L\right )^{T}\right )$

$\\ = \frac{1}{2}\frac{1}{L+k}\left (\left ( L+k \right )\left ( col_{1}Lcol_{1}L^{T} +\cdots +col_{L}Lcol_{L}L^{T}+col_{1}Lcol_{1}L^{T}+\cdots +col_{L}Lcol_{L}L^{T}\right ) \right )\\$

$\\= \frac{1}{2}\frac{1}{L+k}\left (\left ( L+k \right )\left ( 2col_{1}Lcol_{1}L^{T} +\cdots +2col_{L}Lcol_{L}L^{T}\right ) \right )\\ = \frac{1}{2}\frac{1}{L+k}\left (2\left ( L+k \right )LL^{T} \right )\\ =LL^{T}\\ =\Sigma _{xx}$

3、考虑如下离散时间系统

解：

考察教材4.2.3小节的实例应用

（1）整理一下运动方程，求 $F_{k-1}$

看了半天，对 $W_{k}$ 的维度和意义有点迷，可能是考虑到非线性，不管怎样， $W_{k}$ 的维度可以由括号内的加法推测是二维。

因此令：

$W_{k}=\begin{bmatrix} w_{k1}\\ w_{k2} \end{bmatrix}$

则运动方程可以合并为：

$\begin{bmatrix} x_{k}\\ y_{k}\\ \theta _{k} \end{bmatrix} =\begin{bmatrix} x_{k-1}+Tcos\theta _{k-1}(v_{k}+w_{k1})\\ y_{k-1}+Tsin\theta _{k-1}(v_{k}+w_{k1})\\ \theta _{k-1}+T(w_{k}+w_{k2}) \end{bmatrix}$

由教材式（4.25a）得：

$F_{k-1}=\frac{\partial \vec{f} }{\partial\vec{x} }=\begin{bmatrix} \frac{\partial x_{k}}{\partial x_{k-1}} & \frac{\partial x_{k}}{\partial y_{k-1}}& \frac{\partial x_{k}}{\partial \theta _{k-1}}\\ \frac{\partial y_{k}}{\partial x_{k-1}} & \frac{\partial y_{k}}{\partial y_{k-1}}& \frac{\partial y_{k}}{\partial \theta _{k-1}}\\ \frac{\partial \theta _{k}}{\partial x_{k-1}} & \frac{\partial \theta_{k}}{\partial y_{k-1}}& \frac{\partial \theta_{k}}{\partial \theta _{k-1}}\\ \end{bmatrix} =\begin{bmatrix} 1 & 0 & -Tsin\theta _{k-1}(v_{k}+w_{k1})\\ 0& 1 &Tcos\theta _{k-1}(v_{k}+w_{k1}) \\ 0 & 0 & 1 \end{bmatrix}$

雅克比矩阵 $F_{k-1},G_{k}$ 的求解涉及到矩阵的求导方法，所以仍然需要打野发育：矩阵导数

教材中一直采用的是矩阵导数链接中的第4种，很多地方省略了转置符号，高博也有说，即：

由教材式（4.25b）得：

$\\w_{k}{}'=\frac{\partial \vec{f}}{\partial \vec{w_{k}}}w_{k} \\=\begin{bmatrix} \frac{\partial x_{k}}{\partial w_{k1}} &\frac{\partial x_{k}}{\partial w_{k2}} \\ \frac{\partial y_{k}}{\partial w_{k1}} &\frac{\partial y_{k}}{\partial w_{k2}} \\ \frac{\partial \theta _{k}}{\partial w_{k1}} &\frac{\partial \theta_{k}}{\partial w_{k2}} \end{bmatrix} \begin{bmatrix} w_{k1}\\ w_{k2} \end{bmatrix} \\=\begin{bmatrix} Tcos\theta _{k-1} &0 \\ Tsin\theta _{k-1} &0 \\ 0& T \end{bmatrix} \begin{bmatrix} w_{k1}\\ w_{k2} \end{bmatrix} \\=\begin{bmatrix} Tw_{k1}cos\theta _{k-1}\\ Tw_{k1}sin\theta _{k-1}\\ Tw_{k2} \end{bmatrix}$

同理可由教材式（4.26）得：

由于 $n_{k}$ 是直接加到观测方程后面的，所以是线性的，则有：

$n_{k}=n_{k}{}'$

由教材式（4.17）得：

$\\Q_{k}{}'=E[w_{k}{}'w_{k}{}'^{T}] \\=\begin{bmatrix} Tcos\theta _{k-1} &0 \\ Tsin\theta _{k-1} &0 \\ 0& T \end{bmatrix}E[w_{k}w_{k}^{T}] \begin{bmatrix} Tcos\theta _{k-1} &0 \\ Tsin\theta _{k-1} &0 \\ 0& T \end{bmatrix}^{T} \\=\begin{bmatrix} Tcos\theta _{k-1} &0 \\ Tsin\theta _{k-1} &0 \\ 0& T \end{bmatrix}Q_{k} \begin{bmatrix} Tcos\theta _{k-1} &0 \\ Tsin\theta _{k-1} &0 \\ 0& T \end{bmatrix}^{T}$