Where Can Machine Learning Help Robotic State Estimation 机器学习在机器人状态估计的应用

Where Can Machine Learning Help Robotic State Estimation

Tim Barfoot 关于机器学习在机器人状态估计中应用的报告演讲。演讲时间2021年11月。视频链接: bilibili youtube

关键词：高斯过程回归、变分推理、期望最大化、深度神经网络、核方法、Koopman 嵌入、随机傅里叶特征

Barfoot’s lab

research insterests: localization, mapping, planning, control (on UGV)

state estimation is a core capability needed to enable downstream tasks such as planning and control.

Continuous-Time Trajectories

$$\begin{array}{rl}\text{motion model: }& \dot{\mathbf{x}}(t)=\mathbf{A}(t)\mathbf{x}(t)+\mathbf{v}(t)+\mathbf{L}(t)\mathbf{w}(t)\\ \text{observation model: }& {\mathbf{y}}_k={\mathbf{C}}_k\mathbf{x}(t_k)+{\mathbf{n}}_k\end{array}$$
$\mathbf{w}(t)\sim \mathcal{G}\mathcal{P}\left(0,\mathbf{Q}\delta (t-t^{\prime })\right)$, ${\mathbf{n}}_k\sim \mathcal{N}(0,\mathbf{R})$, $\mathbf{x}(t)\sim \mathcal{G}\mathcal{P}\left(\tilde{\mathbf{x}}(t),\tilde{\mathbf{P}}(t,t^{\prime })\right)$

Stochastically integrate the motion model (in closed-form) to create a prior (kernel function)
$$\begin{array}{rl}\mathbf{x}(t)\sim \mathcal{G}\mathcal{P}(& \Phi (t,t_0){\tilde{\mathbf{x}}}_0+{\int }_{t_0}^t\Phi (t,s)\mathbf{v}(s)ds,\\ & \Phi (t,t_0){\tilde{\mathbf{P}}}_0\Phi (t^{\prime },t_0{)}^{\top }+{\int }_{t_0}^{\min (t,t^{\prime })}\Phi (t,s)\mathbf{L}(s)\mathbf{Q}\mathbf{L}(s{)}^{\top }\Phi (t^{\prime },s{)}^{\top }ds)\end{array}$$

Lie groups to handle 3D rotations

[@longBananaDistributionGaussian]

Gaussian variational inference

Use Greek letters to define non-linear model:
$$\begin{array}{rl}\text{motion model: }& {\mathbf{\xi }}_k=\mathrm{f}({\mathbf{\xi }}_{k-1},{\mathbf{\nu }}_k)+{\mathbf{\omega }}_k\\ \text{observation model: }& {\mathbf{\gamma }}_k=\mathrm{g}({\mathbf{\xi }}_k)+{\mathbf{\eta }}_k\end{array}$$
$\xi$ - unknown state, $\nu$ - input, $\omega$ - process niose
$\gamma$ - sensor measurement, $\eta$ - measurement noise

We can simply apply linearization:
$$\begin{array}{rl}\text{motion model: }& {\mathbf{x}}_k={\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{v}}_k+{\mathbf{w}}_k\\ \text{observation model: }& {\mathbf{y}}_k={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{n}}_k\end{array}$$

But it turns out there is another starting point we could think about which is called variation inference. The idea is that we actually want to think about this as an optimization problem, we’d like to set up some data likelihood objective, in this case the log likelihood of all of our measurements, and that could be given some parameters that are associated with our models.

KaTeX parse error: No such environment: split at position 37: …dsymbol} \begin{̲s̲p̲l̲i̲t̲}̲ -\log p(\b\gam…

where $\theta$ - extra unknown parameters, $q(\xi )$ - approximate Gaussian posterior, $\log \left(\frac{p(\xi |\gamma ,\nu ,\theta )}{q(\xi )}\right)$ - true Bayesian posterior.
On rhs, the first term is Kullback-Leibler divergence, which basically talks about the difference between the full Bayesian posterior and our estimate. The second term is the upper bound term.

expectation minimization:

• e-step: hold $\theta$ fixed, minimize $V(q|\theta )$ for $q(\xi )$
• m-step: hold $q(\xi )$ fixed, minimize $V(q|\theta )$ for $\theta$

We improve our state estimation in the e-step and improve our models in the m-step.

KL散度 $\text{KL}(P||Q)={\int }_{-\infty }^{\infty }p(x)\log \frac{p(x)}{q(x)}dx$ . KL散度描述了用一个分布$Q$替代另一个分布$P$时所损失的信息量（熵），$P$和$Q$越相近散度越小。它度量两个概率分布相似度，具有非负性、仿射变换不变性、非对易性等。链接

upper bound:
$$\begin{gathered} V(q|\theta )=-\int q(\mathbf{\xi })\log \left(\frac{p(\mathbf{\xi },\mathbf{\gamma }|\mathbf{\nu },\mathbf{\theta })}{q(\mathbf{\xi })}\right)d\mathbf{\xi } \\ ={\mathbb{E}}_q[-\log p(\mathbf{\xi },\mathbf{\gamma }|\mathbf{\nu },\mathbf{\theta })]+\frac{1}{2}\log |{\mathbf{\Sigma }}^{-1}| \end{gathered}$$

e-step (state estimation):

• find the best mean and (inverse) covariance $q(\mathbf{\xi })=\mathcal{N}(\mathbf{\mu },\mathbf{\Sigma })$
• exactly sparse Gaussian variational inference [@barfootExactlySparseGaussian2020]

m-step (unsupervised parameter learning)

• without any groundtruth fot the trajectory, we can learn almost any parameter of the system:
  $$\begin{array}{rl}{\mathbf{x}}_k& =\mathbf{A}{\mathbf{x}}_{k-1}+{\mathbf{v}}_k+{\mathbf{w}}_k\\ {\mathbf{y}}_k& =\mathbf{C}{\mathbf{x}}_k+{\mathbf{n}}_k\\ {\mathbf{w}}_k& \sim \mathcal{N}(0,\mathbf{Q})\\ {\mathbf{n}}_k& \sim \mathcal{N}(0,\mathbf{R})\end{array}$$

[@yoonUnsupervisedLearningLidar2021]

Kernel Embedding

Every low-demensional nonlinear system can always be lifted up and written as some linear differential equation in higher demensional space (maybe infinte dimensional).
$$\dot{\xi }=f(\xi )\Rightarrow \dot{\mathbf{x}}=\mathbf{A}\mathbf{x}$$

In state estimation:
$$\begin{array}{rl}{\mathbf{\xi }}_k& =\mathrm{f}({\mathbf{\xi }}_{k-1},{\mathbf{\nu }}_k)+{\mathbf{\omega }}_k\\ {\mathbf{\gamma }}_k& =\mathrm{g}({\mathbf{\xi }}_k)+{\mathbf{\eta }}_k\end{array}$$

apply restrict class to control-affine systems:
$$\begin{array}{rl}{\mathbf{\xi }}_k& ={\mathrm{f}}_0({\mathbf{\xi }}_{k-1})+\sum _i{\mathrm{f}}_i({\mathbf{\xi }}_{k-1}){\mathbf{\nu }}_{k,i}+{\mathbf{\omega }}_k\\ {\mathbf{\gamma }}_k& =\mathrm{g}({\mathbf{\xi }}_k)+{\mathbf{\eta }}_k\end{array}$$
embed as a high-dimensional, time-invariant bilinear system [@bruderAdvantagesBilinearKoopman2021]
$$\begin{array}{rl}& {\mathbf{x}}_k=\mathbf{A}{\mathbf{x}}_{k-1}+\mathbf{B}{\mathbf{u}}_k+\mathbf{H}({\mathbf{u}}_k\otimes {\mathbf{x}}_{k-1})+{\omega }_k\\ & {\mathbf{y}}_k=\mathbf{C}{\mathbf{x}}_k+{\mathbf{n}}_k\end{array}$$
rearrange as a high-dimensional, time-varying, linear system. [@guoKoopmanLinearizationDataDriven2022]
$$\begin{array}{rl}\text{Let }{\mathbf{A}}_{k-1}& =\mathbf{A}+\mathbf{H}({\mathbf{u}}_k\otimes 1),{\mathbf{v}}_k=\mathbf{B}{\mathbf{u}}_k\\ \Rightarrow {\mathbf{x}}_k& ={\mathbf{A}}_{k-1}{\mathbf{x}}_{k-1}+{\mathbf{v}}_k+{\mathbf{w}}_k\\ {\mathbf{y}}_k& ={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{n}}_k\end{array}$$
Solve the batch linear-Gaussian state estimation equations
$$\underset{{\hat{\mathbf{P}}}^{-1}}{\underbrace{({\mathbf{A}}^{-\top }{\mathbf{Q}}^{-1}{\mathbf{A}}^{-1}+{\mathbf{C}}^{\top }{\mathbf{R}}^{-1}\mathbf{C})}}\hat{\mathbf{x}}={\mathbf{A}}^{-\top }{\mathbf{Q}}^{-1}\mathbf{v}+{\mathbf{C}}^{\top }{\mathbf{R}}^{-1}\mathbf{y}$$
collapse the high-dimensial estimate to the original space using the Representer Theorem

Others

• Match UAV images to Satelite images.
• Learning features for long-term visual localization, match images across severe lighting change.
• Semantically segmenting classes.

参考文献

[1] LONG A W. WOLFE K C. MASHNER M J. 等. The
Banana Distribution is Gaussian: A Localization Study with Exponential
Coordinates[J]. 8.

[2] BARFOOT T D. FORBES J R. YOON D. Exactly Sparse
Gaussian Variational Inference with Application to Derivative-Free Batch
Nonlinear State Estimation[M/OL]. arXiv, 2020[2022-07-05].
http://arxiv.org/abs/1911.08333.

[3] YOON D J. ZHANG H. GRIDSETH M. 等. Unsupervised
Learning of Lidar Features for Use in a Probabilistic Trajectory
Estimator[M/OL]. arXiv, 2021[2022-07-05].
http://arxiv.org/abs/2102.11261.

[4] BRUDER D. FU X. VASUDEVAN R. Advantages of
Bilinear Koopman Realizations for the Modeling and Control of Systems
With Unknown Dynamics[J/OL]. IEEE Robotics and Automation Letters,
2021, 6(3): 4369-4376.
DOI:10.1109/LRA.2021.3068117.

[5] GUO Z C. KOROTKINE V. FORBES J R. 等. Koopman
Linearization for Data-Driven Batch State Estimation of Control-Affine
Systems[J/OL]. IEEE Robotics and Automation Letters, 2022, 7(2):
866-873.
DOI:10.1109/LRA.2021.3133587.