ORB-EKF

前言

ORB是视觉slam算法，这里利用ORB提取到的特征点用于EKF，本实验模型为车载RGBD相机

---

1. 运动方程

与里程计模型相同，但是区别于传统的平面机器人姿态估计，状态向量中存储的landmark是三维的，即：
$state\_vector=\left[\begin{array}{c}x\\ y\\ \theta \\ L_i^{world}\end{array}\right]$

其中：
$L_i^{world}=\left[\begin{array}{c}{L}_x^{world}\\ L_y^{world}\\ L_z^{world}\end{array}\right]$

所以协方差矩阵的维度：
$size(P)=3+numlandmark\ast 3,3+numlandmark\ast 3$

2. 观测方程（observation function）

状态向量$state\_vector$中存储是地标的世界坐标和相机的世界坐标与姿态
那么引入两个算式：
1）landmark由世界坐标到相机坐标：
$r_i=\left[\begin{array}{c}x\\ y\\ 0\end{array}\right]$

$R=\left[\begin{array}{ccc}sin(\theta )& -cos(\theta )& 0\\ 0& 0& -1\\ cos(\theta )& sin(\theta )& 0\end{array}\right]$

$L_i^{camera}=R(L_i^{world}-r_i)$

2) 由相机坐标到像素坐标
$u=c_x+f_x{L}_x^{camera}/L_z^{camera}$
$v=c_y+f_y{L}_y^{camera}/L_z^{camera}$

将上述两个算式组合就可以得到观测方程：
$u=c_x+\frac{f_x(sin(\theta )(l_x^k-x)-cos(\theta )(l_y^k-y))}{cos(\theta )(l_x^k-x)+sin(\theta )(l_y^k-y)}$

$v=c_y-\frac{l_z^k{f}_y}{cos(\theta )(l_x^k-x)+sin(\theta )(l_y^k-y)}$

我们认为观测为像素坐标

---

3. 逆观测方程 （invers observation function）

2.中已经讨论过观测方程，由$L_x^{world},L_y^{world},L_z^{world}$得到像素坐标$u,v$,那么所谓的逆运动方程就是反过来：由$u,v+d$(d
表示深度，其实就等于$L_z^{world}$),计算$L_x^{world},L_y^{world}$，总之：

$l_x^k=dcos(\theta )+\frac{(u-c_x)dsin(\theta )}{f_x}+x$

$l_k^y=dsin(\theta )-\frac{(u-c_x)dcos(\theta )}{f_x}+y$

$l_k^z=-\frac{(v-c_y)d}{f_y}$

---

3. 雅各比矩阵H

3.1 observation function Jacob to pose

$d=cos(\theta )(l_x^k-x)+sin(\theta )(l_y^k-y)$

$\frac{\partial d}{\partial x}=-cos(\theta )$

$\frac{\partial d}{\partial y}=-sin(\theta )$

$\frac{\partial d}{\partial \theta }=-sin(\theta )(l_x^k-x)+cos(\theta )(l_y^k-y)$

$H_{pose}=\left[\begin{array}{ccc}\frac{\partial u}{\partial x}& \frac{\partial u}{\partial y}& \frac{\partial u}{\partial \theta }\\ \frac{\partial v}{\partial x}& \frac{\partial v}{\partial y}& \frac{\partial v}{\partial \theta }\end{array}\right]$

那么有：

$H_{pose}=\left[\begin{array}{ccc}\frac{f_x(l_y^k-y)}{(cos(\theta )(l_x^k-x)+sin(\theta )(l_y^k-y){)}^2}& \frac{-f_x(l_x^k-x)}{(cos(\theta )(l_x^k-x)+sin(\theta )(l_y^k-y){)}^2}& \frac{-f_x((l_y^k-y{)}^2+(l_x^k-x{)}^2)}{(cos(\theta )(l_x^k-x)+sin(\theta )(l_y^k-y){)}^2}\\ \frac{cos(\theta )f_y{l}_z^k}{(cos(\theta )(l_x^k-x)+sin(\theta )(l_y^k-y){)}^2}& \frac{sin(\theta )f_y{l}_z^k}{(cos(\theta )(l_x^k-x)+sin(\theta )(l_y^k-y){)}^2}& -\frac{f_y{l}_z^k(-sin(\theta )(l_x^k-x)+cos(\theta )(l_y^k-y))}{(cos(\theta )(l_x^k-x)+sin(\theta )(l_y^k-y){)}^2}\end{array}\right]$

$H_{pose}$搞错了，每一项都要加上一个负号

---

3.2 observation function Jacob to landmark

$d=cos(\theta )(l_x^k-x)+sin(\theta )(l_y^k-y)$

$\frac{\partial d}{\partial l_x^k}=cos(\theta )$

$\frac{\partial d}{\partial l_y^k}=sin(\theta )$

$H_{map}=\left[\begin{array}{ccc}\frac{\partial u}{\partial l_x^k}& \frac{\partial u}{\partial l_y^k}& \frac{\partial u}{\partial l_z^k}\\ \frac{\partial v}{\partial l_x^k}& \frac{\partial v}{\partial l_y^k}& \frac{\partial v}{\partial l_z^k}\end{array}\right]$

那么有：

$H_{map}=\left[\begin{array}{ccc}-\frac{f_x(l_y^k-y)}{cos(\theta )(l_x^k-x)+sin(\theta )(l_y^k-y)}& \frac{f_x(l_x^k-x)}{cos(\theta )(l_x^k-x)+sin(\theta )(l_y^k-y)}& 0\\ -\frac{l_z^k{f}_{y}cos(\theta )}{(cos(\theta )(l_x^k-x)+sin(\theta )(l_y^k-y){)}^2}& -\frac{l_z^k{f}_{y}sin(\theta )}{(cos(\theta )(l_x^k-x)+sin(\theta )(l_y^k-y){)}^2}& \frac{f_y}{cos(\theta )(l_x^k-x)+sin(\theta )(l_y^k-y)}\end{array}\right]$

$H_{map}$搞错了，每一项都要加上一个负号

---

3.3 inverse observation function Jacob to pose

$G_{pose}=\left[\begin{array}{ccc}1& 0& -dsin(\theta )+\frac{(u-c_x)dcos(\theta )}{f_x}\\ 0& 1& dcos(\theta )+\frac{(u-c_x)dsin(\theta )}{f_x}\\ 0& 0& 0\end{array}\right]$

---

3.4 inverse observation function Jacob to landmark

$G_{map}=\left[\begin{array}{cc}\frac{dsin(\theta )}{f_x}& 0\\ -\frac{dcos(\theta )}{f_x}& 0\\ 0& -\frac{d}{f_y}\end{array}\right]$