如何在有噪声的数据中进行准确的状态估计?
根据第二讲的运动和观测模型
x k = f ( x k − 1 , u k ) + w k x_{k}=f(x_{k-1},u_{k})+w_{k} xk=f(xk−1,uk)+wk z k , j = h ( y j , x k ) + v k , j z_{k,j}=h(y_{j},x_{k})+v_{k,j} zk,j=h(yj,xk)+vk,j
其中, w k , v k , j w_{k},v_{k,j} wk,vk,j为噪声
根据第五讲的李代数 ,可以将观测方程改写为: s z k , j = K e x p ( ξ ^ ) y i sz_{k,j}=Kexp(\hat\xi)y_i szk,j=Kexp(ξ^)yi
假设 w k , v k , j w_{k},v_{k,j} wk,vk,j噪声服从零均值高斯分布
w k ∼ N ( 0 , R k ) , v k ∼ N ( 0 , Q k , j ) w_k \sim N(0,R_k),v_k \sim N(0,Q_{k,j}) wk∼N(0,Rk),vk∼N(0,Qk,j)
问题:所有未知量放在状态变量中 x = { x 1 , . . . , x N , y 1 , . . . , y M } x=\{x_1,...,x_N,y_1,...,y_M\} x={x1,...,xN,y1,...,yM},
已知输入 u u u 和观测数据 z z z ,求解 x x x 的条件概率分布:
P ( x ∣ z , u ) P(x|z,u) P(x∣z,u)若没有运动,只有观测,则为 P ( x ∣ z ) P(x|z) P(x∣z).根据贝叶斯法则,且 P ( z ) P(z) P(z)与 x x x无关,所以:
P ( x ∣ z ) = P ( z ∣ x ) P ( x ) P ( z ) ∝ P ( z ∣ x ) P ( x ) P(x|z)=\frac{P(z|x)P(x)}{P(z)}\propto P(z|x)P(x) P(x∣z)=P(z)P(z∣x)P(x)∝P(z∣x)P(x) 后 验 = 似 然 ∗ 先 验 后验 = 似然*先验 后验=似然∗先验后验: P ( x ∣ z ) P(x|z) P(x∣z):在 观测值 z z z 发生的情况下,出现 状态 x x x 的概率
似然: P ( z ∣ x ) P(z|x) P(z∣x):在 z z z 发生的情况下,出现 x x x 的概率
先验: P ( x ) P(x) P(x)
问题转化为后验概率最大化MAP:
x M A P ∗ = a r g m a x P ( x ∣ z ) = a r g m a x P ( z ∣ x ) P ( x ) x^*_{MAP} = arg max P(x|z) = arg max P(z|x)P(x) xMAP∗=argmaxP(x∣z)=argmaxP(z∣x)P(x)若先验 P ( x ) P(x) P(x)未知,则问题转化为最大似然估计MAP(Maximize a Posterior):
x M L E ∗ = a r g m a x P ( z ∣ x ) x^*_{MLE} = arg max P(z|x) xMLE∗=argmaxP(z∣x)意义:在什么样的状态下,最有可能产生现在观测到的数据。
经过一系列推导…看书去
问题转化为最小化二次型的问题:
x ∗ = a r g m i n ( ( z k , j − h ( x k , y j ) ) T Q k , j − 1 ( ( z k , j − h ( x k , y j ) ) ) x^* = arg min((z_{k,j}-h(x_k,y_j))^TQ^{-1}_{k,j}((z_{k,j}-h(x_k,y_j))) x∗=argmin((zk,j−h(xk,yj))TQk,j−1((zk,j−h(xk,yj)))因此对于所有的运动和任意的观测,定义数据与估计值之间的误差为
e v , k = x k − f ( x k − 1 , u k ) e_{v,k} = x_k - f(x_{k-1},u_k) ev,k=xk−f(xk−1,uk) e y , j , k = z k , j − h ( x k , y j ) e_{y,j,k} = z_{k,j} - h(x_{k},y_j) ey,j,k=zk,j−h(xk,yj)求该误差的平方和:
J ( x ) = ∑ k e v , k T R k − 1 e v , k + ∑ k ∑ j e y , k , j T Q k , j − 1 e y , k , j J(x) = \sum_{k}^{}e^{T}_{v,k}R^{-1}_{k}e_{v,k}+\sum_{k}\sum_{j}e^{T}_{y,k,j}Q^{-1}_{k,j}e_{y,k,j} J(x)=k∑ev,kTRk−1ev,k+k∑j∑ey,k,jTQk,j−1ey,k,j w k ∼ N ( 0 , R k ) , v k ∼ N ( 0 , Q k , j ) w_k \sim N(0,R_k),v_k \sim N(0,Q_{k,j}) wk∼N(0,Rk),vk∼N(0,Qk,j)于是得到最小二乘问题(Least Square Problem),最小二乘的最优解等价于状态的最大似然估计。
非线性最小二乘:
m i n x ∣ ∣ f ( x ) ∣ ∣ 2 2 \underset{x}{min} ||f(x)||_{2}^{2} xmin∣∣f(x)∣∣22直接求该函数的导数,找使之最小的 x x x,不好算。方法:迭代,不断寻找梯度并下降
1.给定某个初始值 x 0 x_0 x0
2.对于第k次迭代,寻找一个增量 Δ x k \Delta x_{k} Δxk,使得 ∣ ∣ f ( x k + Δ x k ) ∣ ∣ 2 2 ||f(x_{k}+\Delta x_{k})||_{2}^{2} ∣∣f(xk+Δxk)∣∣22达到极小
3.若 Δ x k \Delta x_{k} Δxk足够小,则停止
4.否则,令 x k + 1 = x k + Δ x k x_{k+1}= x_{k}+ \Delta x_{k} xk+1=xk+Δxk,返回第2步
对 f ( x ) 2 f(x)^2 f(x)2进行泰勒展开:
∣ ∣ f ( x k + Δ x k ) ∣ ∣ 2 2 ≈ ∣ ∣ f ( x ) ∣ ∣ 2 2 + J ( x ) Δ x k + 1 2 Δ x k T H Δ x k ||f(x_{k}+\Delta x_{k})||_{2}^{2} \approx||f(x)||_{2}^{2}+J(x)\Delta x_{k}+ \frac{1}{2}\Delta x_{k}^TH\Delta x_{k} ∣∣f(xk+Δxk)∣∣22≈∣∣f(x)∣∣22+J(x)Δxk+21ΔxkTHΔxk J J J:雅各比矩阵, ∣ ∣ f ( x ) ∣ ∣ 2 2 ||f(x)||_{2}^{2} ∣∣f(x)∣∣22一阶导;
H H H:海塞矩阵, ∣ ∣ f ( x ) ∣ ∣ 2 2 ||f(x)||_{2}^{2} ∣∣f(x)∣∣22二阶导
Δ x = − J T ( x ) \Delta x = -J^T(x) Δx=−JT(x)意义:沿着反向梯度方向前进 ,通常计算该方向上的一个ie步长,求得最快的下降方式
H Δ x = − J T H\Delta x = -J^T HΔx=−JT
对 f ( x ) f(x) f(x)进行泰勒展开:
f ( x + Δ x ) ≈ f ( x ) + J ( x ) Δ x f(x+\Delta x) \approx f(x)+J(x)\Delta x f(x+Δx)≈f(x)+J(x)Δx J J J:雅各比矩阵, f ( x ) f(x) f(x)关于x的一阶导;
最小二乘问题转化为 : 求下降矢量 Δ x \Delta x Δx,使 ∣ ∣ f ( x + Δ x k ) ∣ ∣ 2 ||f(x+\Delta x_{k})||^{2} ∣∣f(x+Δxk)∣∣2最小。
Δ x = a r g m i n Δ x ∣ ∣ f ( x ) + J ( x ) Δ x ∣ ∣ 2 \Delta x = arg\underset{\Delta x}{min} ||f(x)+J(x)\Delta x||^{2} Δx=argΔxmin∣∣f(x)+J(x)Δx∣∣2将函数展开,令其关于 Δ x \Delta x Δx的导数为0,得到增量方程(高斯牛顿方程、正规方程):
J ( x ) T J ( x ) Δ x = J ( x ) T f ( x ) J(x)^TJ(x)\Delta x=J(x)^Tf(x) J(x)TJ(x)Δx=J(x)Tf(x)简化写为
H Δ x = g H\Delta x = g HΔx=g比起牛顿法求二阶导数的H,高斯牛顿法的H算到一阶导数乘法即可。
高斯牛顿法的关键在于求解增量方程
算法步骤:
1.给定某个初始值 x 0 x_0 x0
2.对于第k次迭代,计算当前的 J ( x k ) J(x_{k}) J(xk)和 f ( x k ) f(x_{k}) f(xk)
3.求解增量方程 H Δ x = g H\Delta x = g HΔx=g
4.若 Δ x \Delta x Δx足够小,则停止;否则,令 x k + 1 = x k + Δ x k x_{k+1}= x_{k}+ \Delta x_{k} xk+1=xk+Δxk,返回第2步
缺点:实际求解中的 H H H 不是正规矩阵,算法稳定性差且不收敛。
信赖区域方法:泰勒展开只能在展开点附近有较好的近似效果,故需要给 Δ x \Delta x Δx添加一个信赖区域(Trust Region),认为信赖区域内近似有效。
信赖区域的确定:根据近似模型和实际函数之间的差异确定,若差异小,则让范围大;若差异大,则让范围小。
ρ = f ( x + Δ x ) − f ( x ) J ( x ) Δ x \rho = \frac{f(x+\Delta x)-f(x)}{J(x)\Delta x} ρ=J(x)Δxf(x+Δx)−f(x)
f ( x + Δ x ) − f ( x ) f(x+\Delta x)-f(x) f(x+Δx)−f(x) : 实际下降值
J ( x ) Δ x J(x)\Delta x J(x)Δx : 近似下降值
若 ρ \rho ρ 近似 1 1 1,效果好;
若 ρ \rho ρ 太小,采用的近似下降太大,需要缩小近似范围;
若 ρ \rho ρ 太大,采用的近似下降太小,需要扩大近似范围;
算法步骤:
1.给定初始值 x 0 x_0 x0,初始优化半径 μ \mu μ
2.对于第k次迭代,求解 Δ x k = a r g m i n Δ x k ∣ ∣ f ( x k ) + J ( x k ) Δ x k ∣ ∣ 2 , s . t . ∣ ∣ D Δ x k ∣ ∣ ⩽ μ \Delta x_{k} = arg\underset{\Delta x_{k}}{min} ||f(x_{k})+J(x_{k})\Delta x_{k}||^{2}, s.t. ||D\Delta x_{k}||\leqslant \mu Δxk=argΔxkmin∣∣f(xk)+J(xk)Δxk∣∣2,s.t.∣∣DΔxk∣∣⩽μ3.计算 ρ \rho ρ
4.若 ρ \rho ρ > 3 4 , 则 μ = 2 μ \frac{3}{4},则 \mu=2\mu 43,则μ=2μ
5.若 ρ \rho ρ < 1 4 , 则 μ = 0.5 μ \frac{1}{4},则 \mu=0.5\mu 41,则μ=0.5μ
6.若 ρ \rho ρ > threshold,则近似可行,令 x k + 1 = x k + Δ x k x_{k+1}= x_{k}+ \Delta x_{k} xk+1=xk+Δxk
7.判断算法是否收敛,不收敛则返回第二步,否则结束
将有约束的问题转为无约束的问题 (拉格朗日乘子 λ \lambda λ)
Δ x k = a r g m i n Δ x k ∣ ∣ f ( x k ) + J ( x k ) Δ x k ∣ ∣ 2 , s . t . ∣ ∣ D Δ x k ∣ ∣ ⩽ μ \Delta x_{k} = arg\underset{\Delta x_{k}}{min} ||f(x_{k})+J(x_{k})\Delta x_{k}||^{2}, s.t. ||D\Delta x_{k}||\leqslant \mu Δxk=argΔxkmin∣∣f(xk)+J(xk)Δxk∣∣2,s.t.∣∣DΔxk∣∣⩽μ
Δ x k = a r g m i n Δ x k ∣ ∣ f ( x k ) + J ( x k ) Δ x k ∣ ∣ 2 + λ 2 ∣ ∣ D Δ x k ∣ ∣ \Delta x_{k} = arg\underset{\Delta x_{k}}{min} ||f(x_{k})+J(x_{k})\Delta x_{k}||^{2}+\frac{\lambda}{2} ||D\Delta x_{k}|| Δxk=argΔxkmin∣∣f(xk)+J(xk)Δxk∣∣2+2λ∣∣DΔxk∣∣化为增量方程的形式:
( H + λ D T D ) Δ x = g (H+\lambda D^TD)\Delta x = g (H+λDTD)Δx=g
Levenberg设D为单位阵,将 Δ x \Delta x Δx约束在球里
Marquardt设D为非负数对角阵,实际中常用 J T J J^TJ JTJ的对角元素平方根。
( H + λ I ) Δ x = g (H+\lambda I)\Delta x = g (H+λI)Δx=g
λ \lambda λ小,二次近似效果好,LM法类似高斯牛顿法
λ \lambda λ大,二次近似效果差,LM法类似一阶梯度下降
已知方程曲线 y = e x p ( a x 2 + b x + c ) + w y=exp(ax^2+bx+c)+w y=exp(ax2+bx+c)+w的N个关于x,y的观测点,如何求a,b,c?
将问题转化为求解最小二乘问题:
m i n a , b , c ∣ ∣ y i − e x p ( a x i 2 + b x i + c ) ∣ ∣ 2 \underset{a,b,c}{min} ||y_i-exp(ax_{i}^2+bx_{i}+c)||^{2} a,b,cmin∣∣yi−exp(axi2+bxi+c)∣∣2
第一步:定义一个代价函数的结构体struct
第二步:增加一个残差块(Problem.AddResidualBlock)将误差项加入到问题中
第三步:指定误差项和优化变量的纬度
第四步:问题求解(Problem.Solve)
cmake_minimum_required(VERSION 3.14)
project(useceres)
set(CMAKE_CXX_STANDARD 14)
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )
find_package( Ceres REQUIRED )
include_directories( ${CERES_INCLUDE_DIRS} )
include_directories( /usr/include/eigen3)
include_directories( /usr/local/include/ceres)
add_executable(useceres main.cpp)
target_link_libraries( useceres ${OpenCV_LIBS} ${CERES_LIBRARIES} )
#include
#include
#include
using namespace std;
// 代价函数的计算模型
struct CURVE_FITTING_COST{
CURVE_FITTING_COST (double x,double y):_x(x),_y(y) {}
template <typename T>
bool operator()(const T* const abc, T* residual ) const{
//y-exp(ax^+bx+c)
residual[0] = T(_y) -ceres::exp(abc[0]*T(_x)*T(_x)+ abc[1]*T(_x) + abc[2]);
return true;
}
const double _x,_y;
};
int main() {
double a=1.0, b=2.0, c=1.0; // abc真实值
int N=100;
double v_sigma = 1.0;
cv::RNG rng;//opencv随机数产生器
double abc[3] = {0,0,0}; // abc估计值
// 用高斯噪声,产生一百对随机数据
vector<double > x_data, y_data; //数据
cout << "generating data: "<<endl;
for (int i = 0; i < N; ++i) {
double x = i/100.0;
x_data.push_back(x);
y_data.push_back(exp(a*x*x+b*x+c)+rng.gaussian(v_sigma));
}
// 构建最小二乘问题
ceres::Problem problem;
for (int i = 0; i < N; ++i) {
problem.AddResidualBlock( //向问题中添加误差项
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST( x_data[i], y_data[i])),
nullptr, //核函数,此处不使用
abc // 待估计参数
);
}
//配置求解器
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR; // 增量方程采用QR求解方式
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary; //优化信息
ceres::Solve( options, &problem, &summary); //开始优化
cout<< summary.BriefReport()<<endl; //输出结果
cout<< "estimated a,b,c = ";
for( auto a:abc) cout<< a<< " ";
cout << endl;
return 0;
}
查看结果
build/curve_fitting
图优化:是把优化问题表现成图的形式,通常分解为两个任务:
核心(SparseOptimizer):是OptimizableGraph,也是HyperGraph(超图)。
顶点(HyperGraph::Vertex):优化变量,继承于OptimizableGraph::Vertex(也是Base Vertex)。
边(HyperGraph::Edge):误差项,继承于OptimizableGraph::Edge(也是单边(BaseUnaryEdge)、双边(BaseBinaryEdge)、多边(BaseMultiEdge))。
优化算法OptimizationAlgorithm是通过OptimizationWithHessian实现的,其中有迭代方法GaussNewton,LM,Powell’s dogleg。
求解器Solver:由BlockSolver组成,其中BlockSolver包含两部分,分别是:
SparseBlockMatrix:用于计算稀疏的雅可比矩阵 J J J 和Hessian矩阵 H H H
LinearSolver:是线性求解器,用于计算迭代过程中最关键的 H Δ x = − b H\Delta x=-b HΔx=−b,方法包括有PCG,CSparse,Choldmod
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
// 曲线模型的顶点,模板参数:优化变量纬度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl()
{
_estimate << 0,0,0;
}
virtual void oplusImpl( const double* update )
{
_estimate += Eigen::Vector3d(update);
}
//存盘和读盘: 留空
virtual bool read( istream& in ) {}
virtual bool write( ostream& out) const{}
};
// 误差模型 模板参数: 观测值纬度, 类型, 连接点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
//计算曲线模型误差
void computeError(){
const CurveFittingVertex* v = static_cast<const CurveFittingVertex* >(_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x +abc(1,0)*_x + abc(2,0));
}
virtual bool read (istream& in){}
virtual bool write( ostream& out) const {};
public:
double _x;
};
int main() {
double a=1.0, b=2.0, c=1.0;
int N=100;
double w_sigma = 1.0;
cv::RNG rng;
double abc[3]= {0,0,0};
vector<double> x_data, y_data;
cout << "generating data: "<<endl;
for (int i = 0; i < N; ++i) {
double x = i/100.0;
x_data.push_back(x);
y_data.push_back( exp(a*x*x+b*x+c ) + rng.gaussian(w_sigma) );
cout << x_data[i] << " " << y_data[i] <<endl;
}
//构建图优化,先设定g2o
// 矩阵块: 每个误差项优化变量纬度为3, 误差值纬度为1
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1>> Block;
std::unique_ptr<Block::LinearSolverType> linearSolver ( new g2o::LinearSolverDense<Block::PoseMatrixType>() );
std::unique_ptr<Block> solver_ptr (new Block( std::move(linearSolver) ));
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::move(solver_ptr) );
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); //设置求解器
optimizer.setVerbose(true); //打开调试输出
//向途中增加顶点
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0));
v->setId(0);
optimizer.addVertex(v);
//向途中增加边
for (int i = 0; i < N; ++i) {
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex(0,v);
edge->setMeasurement( y_data[i] );
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma));
optimizer.addEdge( edge );
}
cout << "start optimization"<<endl;
optimizer.initializeOptimization();
optimizer.optimize(200);
//输出优化值
Eigen::Vector3d abc_estimate = v->estimate();
cout<< "estimate model:" <<abc_estimate.transpose()<<endl;
return 0;
}
// 每个误差项优化变量维度为3,误差值维度为1
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;
// 第1步:创建一个线性求解器LinearSolver
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
// 第2步:创建BlockSolver。并用上面定义的线性求解器初始化
Block* solver_ptr = new Block( linearSolver );
// 第3步:创建总求解器solver。并从GN, LM, DogLeg 中选一个,再用上述块求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
// 第4步:创建终极大boss 稀疏优化器(SparseOptimizer)
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm( solver ); // 设置求解器
optimizer.setVerbose( true ); // 打开调试输出
// 第5步:定义图的顶点和边。并添加到SparseOptimizer中
CurveFittingVertex* v = new CurveFittingVertex(); //往图中增加顶点
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
for ( int i=0; i<N; i++ ) // 往图中增加边
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v ); // 设置连接的顶点
edge->setMeasurement( y_data[i] ); // 观测数值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵:协方差矩阵之逆
optimizer.addEdge( edge );
}
// 第6步:设置优化参数,开始执行优化
optimizer.initializeOptimization();
optimizer.optimize(100);
具体g2o内部详细结构可以参考:
从零开始一起学习SLAM | 理解图优化,一步步带你看懂g2o代码
将位姿估计问题转为最小二乘问题,然后用不同的方法(梯度下降法、高斯牛顿法、LM法)来求解下降的步长。分别用ceres和g2o实现非线性优化。
[1]: Xiang Gao, Tao Zhang, Yi Liu, Qinrui Yan, 14 Lectures on Visual SLAM: From Theory to Practice, Publishing House of Electronics Industry, 2017.
[2]: 视频教程
[3]: 高斯牛顿(Gauss Newton)、列文伯格-马夸尔特(Levenberg-Marquardt)最优化算法
[4]: 深入理解图优化与g2o:图优化篇
[5]:从零开始一起学习SLAM | 理解图优化,一步步带你看懂g2o代码