- slam过程的基本方程：
x k = f ( x k − 1 , u k , w k ) \boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}, \boldsymbol{w}_{k}\right) xk=f(xk−1,uk,wk) （运动方程）
z k , j = h ( y j , x k , v k , j ) \boldsymbol{z}_{k, j}=h\left(\boldsymbol{y}_{j}, \boldsymbol{x}_{k}, \boldsymbol{v}_{k, j}\right) zk,j=h(yj,xk,vk,j) （观测方程）
由此推导出slam问题，已知运动测量的读数u和传感器的读数z，如何求解定位问题（x）和建图问题（y）
- 旋转向量到旋转矩阵的过程为罗德里格斯公式， 如下：
R = cos θ I + ( 1 − cos θ ) n n T + sin θ n ∧ R=\cos \theta I+(1-\cos \theta) n n^{T}+\sin \theta n^{\wedge} R=cosθI+(1−cosθ)nnT+sinθn∧
其反变换如下：
θ = arccos ( tr ( R ) − 1 2 ) \theta=\arccos \left(\frac{\operatorname{tr}(R)-1}{2}\right) θ=arccos(2tr(R)−1)
转轴n为矩阵R的特征向量。给出一个旋转矩阵，即可使用这种方法求出旋转轴和旋转角。 - 欧拉角：欧拉角有多重模式，最常用的是偏航-俯仰-滚转。方法如下：
绕物体的Z 轴旋转，得到偏航角yaw；
绕旋转之后的Y 轴旋转，得到俯仰角pitch；
绕旋转之后的X 轴旋转，得到滚转角roll。 - 四元数：
q = [ cos θ 2 , n x sin θ 2 , n y sin θ 2 , n z sin θ 2 ] T q=\left[\cos \frac{\theta}{2}, n_{x} \sin \frac{\theta}{2}, n_{y} \sin \frac{\theta}{2}, n_{z} \sin \frac{\theta}{2}\right]^{T} q=[cos2θ,nxsin2θ,nysin2θ,nzsin2θ]T
两个相反的四元数表示同一种旋转。 - 罗德里格斯公式实际上是一种指数映射，将旋转向量组成的空间映射到旋转矩阵。
- 李代数（旋转矩阵）通过指数映射变为李群（旋转向量），李群通过对数映射变为李代数。不过通常并不这样做，而是有更加简单的代数方法。
三维旋转的对应变换关系
三维变换的对应变换关系 - 假定对某个旋转R，对应的李代数为ϕ。我们给它左乘一个微小旋转，记作ΔR，对应的李代数为Δϕ，根据bch近似，可以写成：
exp ( Δ ϕ ∧ ) exp ( ϕ ∧ ) = exp ( ( ϕ + J l − 1 ( ϕ ) Δ ϕ ) ∧ ) \exp \left(\Delta \phi^{\wedge}\right) \exp \left(\phi^{\wedge}\right)=\exp \left(\left(\phi+J_{l}^{-1}(\phi) \Delta \phi\right)^{\wedge}\right) exp(Δϕ∧)exp(ϕ∧)=exp((ϕ+Jl−1(ϕ)Δϕ)∧)
反之，则有：
exp ( ( ϕ + Δ ϕ ) ∧ ) = exp ( ( J l Δ ϕ ) ∧ ) exp ( ϕ ∧ ) = exp ( ϕ ∧ ) exp ( ( J r Δ ϕ ) ∧ ) \exp \left((\phi+\Delta \phi)^{\wedge}\right)=\exp \left(\left(J_{l} \Delta \phi\right)^{\wedge}\right) \exp \left(\phi^{\wedge}\right)=\exp \left(\phi^{\wedge}\right) \exp \left(\left(J_{r} \Delta \phi\right)^{\wedge}\right) exp((ϕ+Δϕ)∧)=exp((JlΔϕ)∧)exp(ϕ∧)=exp(ϕ∧)exp((JrΔϕ)∧)
