本文深入探讨SLAM问题中的非线性优化，包括基本概念如高斯分布、矩阵求导和凸优化，以及优化方法如最速下降法、牛顿法、高斯牛顿法和列文伯格-马夸尔特方法。通过最小二乘问题建模，展示了如何将SLAM转化为非线性优化问题，并阐述了最小二乘法和最大似然估计的关系与区别。
我们使用李群李代数解决了SLAM中对位姿求导问题，本文将进一步研究如何使用非线性优化求解SLAM问题。
基本概念
高斯分布
-
概念
若随机变量 x x x服从高斯分布 N ( μ , σ 2 ) N(\mu,\sigma^2) N(μ,σ2)，则它的概率分布函数为
p ( x ) = 1 2 π σ e − 1 2 ( x − μ ) 2 σ 2 p(x)={1 \over \sqrt{2\pi} \sigma}e^{- {1 \over 2} {(x-\mu)^2 \over \sigma^2}} p(x)=2πσ1e−21σ2(x−μ)2
其高维形式为
p ( x ) = 1 ( 2 π ) N ∣ Σ ∣ e − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) p(x)={1 \over \sqrt{(2\pi)^N|\Sigma|}}e^{- {1 \over 2} (x-\mu)^T \Sigma^{-1}(x-\mu)} p(x)=(2π)N∣Σ∣1e−21(x−μ)TΣ−1(x−μ) -
运算性质
设独立的高斯分布：
x ∼ N ( μ x , Σ x x ) , y ∼ N ( μ y , Σ y y ) x \sim N(\mu_x,\Sigma_{xx}),y \sim N(\mu_y,\Sigma_{yy}) x∼N(μx,Σxx),y∼N(μy,Σyy)
则：
x + y ∼ N ( μ x + μ y , Σ x x + Σ y y ) a x ∼ N ( a μ x , a 2 Σ x x ) A X ∼ N ( A μ x , A Σ x x A T ) x+y \sim N(\mu_x+\mu_y,\Sigma_{xx}+\Sigma_{yy}) \\\\ ax \sim N(a\mu_x,a^2\Sigma_{xx}) \\\\ AX \sim N(A\mu_x,A\Sigma_{xx}A^T) x+y∼N(μx+μy,Σxx+Σyy)ax∼N(aμx,a2Σxx)AX∼N(Aμx,AΣxxAT)
若 x x x与 y y y不独立，则其复合分布为
p ( x , y ) = N ( [ μ x μ y ] , [ Σ x x Σ x y Σ y x Σ y y ] ) p(x,y)=N(\begin{bmatrix} \mu_x \\ \mu_y \end{bmatrix},\begin{bmatrix} \Sigma_{xx} & \Sigma_{xy} \\ \Sigma_{yx} & \Sigma_{yy} \end{bmatrix}) p(x,y)=N([μxμy],[ΣxxΣyxΣxyΣyy])
由 p ( x , y ) = p ( x ∣ y ) p ( y ) p(x,y)=p(x|y)p(y) p(x,y)=p(x∣y)p(y)可推出
p ( x ∣ y ) ∼ N ( μ x + Σ x y Σ y y − 1 ( y − μ y ) , Σ x x − Σ x y Σ y y − 1 Σ y x ) p(x|y) \sim N(\mu_x+\Sigma_{xy} {\Sigma_{yy}}^{-1}(y-\mu_y),\Sigma_{xx}-\Sigma_{xy}{\Sigma_{yy}}^{-1}\Sigma_{yx}) p(x∣y)∼N(μx+ΣxyΣyy−1(y−μy),Σxx−ΣxyΣyy−1Σyx)
矩阵求导
-
常用矩阵公式
( A B ) − 1 = B − 1 A − 1 ( A T ) − 1 = ( A − 1 ) T ( A + B ) T = A T + B T ( A B ) T = B T A T A T = A , A 为对称矩阵 T r ( A ) = ∑ i A i i T r ( A ) = ∑ i λ i , λ i = e i g ( A ) T r ( A ) = T r ( A T ) T r ( A B ) = T r ( B A ) T r ( A + B ) = T r ( A ) + T r ( B ) ∣ A ∣ = ∏ i λ i , λ i = e i g ( A ) ∣ c A ∣ = c n ∣ A ∣ , A ∈ R n × n ∣ A T ∣ = ∣ A ∣ ∣ A B ∣ = ∣ A ∣ ∣ B ∣ ∣ A − 1 ∣ = 1 / ∣ A ∣ ∣ A n ∣ = ∣ A ∣ n (AB)^{-1}=B^{-1}A^{-1} \\\\ (A^T)^{-1}=(A^{-1})^T \\\\ (A+B)^T=A^T+B^T \\\\ (AB)^T=B^TA^T \\\\ A^T=A,A为对称矩阵 \\\\ Tr(A)=\sum_iA_{ii} \\\\ Tr(A)=\sum_i\lambda_{i},\lambda_i=eig(A) \\\\ Tr(A)=Tr(A^T) \\\\ Tr(AB)=Tr(BA) \\\\ Tr(A+B)=Tr(A)+Tr(B) \\\\ |A|=\prod_i\lambda_i,\lambda_i=eig(A) \\\\ |cA|=c^n|A|,A \in \R^{n \times n} \\\\ |A^T|=|A| \\\\ |AB|=|A||B| \\\\ |A^{-1}|=1/|A| \\\\ |A^n|=|A|^n \\\\ (AB)−1=B−1A−1(AT)−1=(A−1)T(A+B)T=A
最低0.47元/天 解锁文章
扫一扫
720
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
