把卡尔曼滤波(KF)和批量优化方法(如BA)联系在一起的是“加权最小二乘法”。KF通常是“加权最小二乘法”递归求解过程的forward pass，每个时刻的状态仅与过去时刻的状态相关；而批处理优化方法，每个时刻的状态与过去时刻和之后时刻的状态都有依赖。运动模型(比如常速度或常加速度模型)作为系统的先验信息，对应KF模型的预测步骤，而后验信息通过观测得到更新；有的BA方法不预设模型，比如多视角图像三维重建，状态参数估计方法是用最大似然法。非线性的KF(如EKF)每一时刻对应的线性化求解过程和BA一样，都需要实时计算雅可比(Jacobian)矩阵，高斯消元法(边缘化)求解技巧。

高斯变量线性变换的概率分布

高斯随机变量的分布是 和 分别表示均值向量和协方差矩阵。变量的线性映射为  则变量的分布为。卡尔曼滤波的运动模型(系统先验)中，是转移矩阵，会让状态的协方差增加。

高斯变量的乘积的分布

多个高斯分布的乘积也是高斯分布，，乘积的协方差矩阵和均值与每个单独的高斯分布的关系为：

相乘之后的方差呈减小趋势，均值是单个高斯分布的加权平均。下图是通过两个一维高斯分布说明乘积后高斯分布均值和方差的变化，以及在卡尔曼滤波中，引入观测量如何使得更新后的状态矢量的不确定性变小。

高斯变量的联合分布和条件分布

1. 已知高斯变量和，的分布为  条件分布 ，和的联合分布为

  变量的边缘分布为

2. 已知联合概率

其中。的边缘概率分别为

条件概率为(具体推导过程见[2]的p.19)：

高斯推断(Gaussian inference)，比如先假设的先验，然后根据测量得到的计算出的似然度，可以看出引入测量值使得方差变小。

线性估计的最小二乘法解( From [1])

考虑数据的线性生成模型 若是方阵，的线性估计为  若的行数大于列数，即 ，的线性估计为伪逆解  若，需要引入正则化项，则有

考虑噪声项，由 ，是均值为0协方差矩阵为Q(为正定的已知)的随机噪声，可以推出

的协方差(后验协方差).　

考虑样本权重的加权最小二乘

可以看成是样本具有不同的权重，则表示成加权最小二乘。最小化下面的正定二次目标函数

这时误差项计算的不是欧式距离而是马氏距离(Mahalanobis distances)。估计值满足，得到法方程(Normal equations)：

对应伪逆解(Moore-Penrose pseudoinverse)：

对照前面的公式得到, 是关于的线性变换协方差矩阵，又称为信息矩阵(Information matrix)或精度矩阵(Precision matrix)。部分是否可逆是和系统的可观测性(observability)联系在一起的([2]p.49)。

如果在中加入正则化项则，其中前面的部分和KF中的卡尔曼增益矩阵很像，可以认为是加权情况的观测矩阵的广义逆矩阵。实际上当观测噪声很小，并且迁移矩阵是个方阵时，，这样迭代KF的更新步骤就能直接求解，而不必迭代了。

牛顿法估计

梯度下降法求的根，迭代方程

，

其中是在处求Hessian。可以先解， 再更新。

实际上通过最小化二次目标函数求解x不需要Newton法之类的迭代法，因为此时的Hessian矩阵为是一个与无关的常数，，相当于一步迭代就可求解。

状态空间和状态估计

离散时间线性系统，表达状态传播的预测函数和用于更新当前状态的观测函数：

前一个式子是用上一时刻状态估计当前状态(运动模型，是系统输入，许多时候为0)。第二个式子是根据观测输出对预测进行修正。和是方差为和的噪声项。把已知项都写在右边，状态项写在左边，就能得到一个大的线性估计问题

其中第一行是初始状态，，表示均值。观测的个数m，输入项个数为k，，表示观测函数等式可能比较少，k个时刻只有m个观测量。写成矩阵形式，其中z表示所有k时刻观测量，是的矢量，n表示状态矢量x的维度。那么b是维矢量，A是稀疏矩阵。可以简写成加权最小二乘形式

权重W为Q和R的逆矩阵组成的分块对角阵。这样上边的矩阵可以用前面说的一步迭代法求解。状态估计方法中，未来的测量对当前的估计也有贡献，区别于滤波(filtering)它属于平滑(smoothing)的方法，是一种批处理方法，即所有测量值都用于估计状态。可参见[2]的p.37~43, 3.1 Batch Discrete-Time Estimation一节。A中只有预测部分信息的话，矩阵也可以是满秩的，加入观测量可以修正参数估计值。

上面这个目标函数也可以分解成两个部分：

其中前两项是对应先验部分，包括初始状态和预测量，后一项是观测量部分。视觉领域常用的Bundle Adjustment方法，通常只有观测项。比如Structure from motion应用中，对应的观测项是所有特征点的重投影误差，而不考虑"运动"的先验知识，即相机运动轨迹的平滑特性。

批处理方法快速求解

上面的批处理求解，方程组左边的稀疏的对角分块矩阵，可以用Cholesky分解对应的前推回代法进行快速求解。Cholesky分解能把正定对称阵分解为两个三角阵相乘，，是下三角阵，可以先求解，从第一行往下带入求解称为forward pass，再解，是上三角阵，从最后一行带入求解，称为backward pass。由此导出的Rauch-Tung-Striebel smoother(截图参见[2]的p.58):

这里的是迁移矩阵，是观测矩阵，上标带\check的量如和 表示预测量，带\hat的量和表示更新量，下标的表示forward pass。前推回代法得到的是精确解，如果只进行前推法(forward部分的5个公式)在数学上对应Kalman filtering。

递归最小二乘Recursive least squares

上面的批处理问题无法解在线问题，也就是说当观测量增加时需要再次更新之前时刻的状态。

可以推导出KF的预测和更新公式。[2]中的第三章，提供了更多KF的推导方法。

对于非线性的扩展卡尔曼滤波，过程函数和观测函数都需要线性化，每个时刻迭代多次的iEKF和求解非线性最小二乘法的高斯牛顿法是等价的，GN法迭代过程求解的线性方程组的形式也和上面的类似，，其中A是残差函数的Jacobian。

卡尔曼方法的牛顿法推导

递归过程的目标函数写成：　

SLAM方法中基于滤波的方法的核心是卡尔曼滤波，基于(非线性)优化方法的核心是高斯牛顿法，经由线性化，二者可以通过迭代EKF联系在一起。包含前通和后通步骤的基于平滑的方法等价于最小二乘Cholesky分解的基于优化的方法，是一种批处理方法。

舒尔补（Schur complement）

把分块矩阵通过初等变换变成对角阵，可以通过左右乘以变换矩阵的方法得到：

或　

推出

则的逆矩阵：

其中和分别称为和的舒尔补。有时候，比如是对角矩阵，而且相对于规模较小，用上面的公式求逆，可以大大减小计算量。

计算上面的矩阵相乘：

上下两个分块矩阵对应部分相等，可以得到4个等式，比如利用左上角的块得到：

求逆公式Sherman-Morrison-Woodbury公式可以利用它得到：

这个公式解决的是当矩阵只有一个子块发生了更新——比如递归形式求解最小二乘问题时，正定对称矩阵只有右下角的部分增加了一个对角形式的增量——此时A的逆矩阵如何快速更新的问题。不需要计算逆矩阵，真正需要计算逆矩阵的部分计算效率很高。

用舒尔补求解法方程，简写成，假设矢量可以分解成两个部分，则左边对称的系数矩阵也分解为分块矩阵：

左右都乘以矩阵 ，其实就是高斯消元法把的右上角变成，方程组变成

这样就可以先用：

求出部分，然后再带入，求出部分。在SLAM系统中，通过保留关键帧或滑动窗口来减少要估计的状态变量，通过以上方法来消除非线性优化过程中的状态量的方法，也称为边缘化(marginalization)。比如中包含的旧的相机状态量可以边缘化(marginalized out)，这样中只保留最近关键帧的状态和地图点可以减少求解的计算量[3]。

边缘化可以减小状态参数个数，而conditioning可以增加状态参数个数。边缘化使得原本稀疏的H矩阵变的稠密，原本不相关的观测量之间也建立了连接。在通常SLAM使用的滑窗方法中，相比直接丢弃早先的参数的方法，边缘化方能够保留新旧参数之间的关联。这部分可以参见概率机器人一书[4]和文章[5]。

参考文献:

[1]."Kalman Filtering with Newton's Method"

KF的牛顿法推导：... one-step KF is given by a single Newton method on the gradients of a quadratic objective functions and with a carefully chosen initial guess. It provides a more generic recursive state estimation. can also used for EKF for non linear system.

[2].“State estimation for robotics"  Timothy D. Barfoot

第二章是关于概率的基础，第三章是关于线性高斯变换即卡尔曼的滤波和平滑公式的推导，第四章是关于非线性非高斯滤波，第五章是关于biases, determining correspondences or data association , and detecting/rejecting outliers。KF中一般假设噪声项是零均值高斯量，零均值的假设往往不能满足，存在偏差bias(机器学习中需要建模独立性的时候需要大的bias，比如boosting选取的样本集是有偏的)。bias的例子就是IMU中加速度计和温度相关的时变零偏。第六章是关于多视几何，第七章是关于李代数。

[3].OKVIS:  “Keyframe-Based Visual-Inertial Odometry Using Nonlinear Optimization”　

摘录：视觉-惯性传感器方法可以分为两类。松耦合系统中，如论文10，惯性传感器测量整合进单独的倾角测量计中，非独立、相对的航向测量整合到立体视觉优化里面。论文20，采用单视觉位姿估计作为数据更新，给到具有非直接IMU数据的EKF方法中。论文15,7中，相对的立体位姿估计整合进一个因子图中，它包含惯性数据，和绝对GPS测量数据。这些方法都不复杂，但忽略了不同传感器的内部相关性。相反，紧耦合方法，充分考虑了所有传感器的状态。为了便于追踪，用于取代滤波方法，论文2，提出了一个固定延时的平滑器，它有一个窗口提供连续的机器人位姿和相对状态，删除规定范围以外的的状态，如论文19。论文16，采用类似的方法，但没有惯性传感器，工作在平面环境。

考虑到视觉-惯性传感器SLAM的精度和鲁棒性，我们倾向于紧耦合的融合方法，可以最大程度地利用感知的信息和非线性估计方法，而不是滤波方法，只是部分优化的线性方法。我们的方法是受到论文17的启发，它建议用将IMU的误差放到batch-optimized SLAM处理（只在初始化时）。我们的方法采用论文2中固定延迟平滑器，它集成了惯性传感器，用一个代价函数重映射误差，为了控制计算复杂度，老的状态会被丢弃。

[4]."Probabilistic Robotics" 第11章关于GraphSLAM.

[5]."A Sliding Window Filter for SLAM"

... it is not surprising that marginalizing out parameters induces conditional dependencies between all the remaining parameters that the removed parameters were conditionally dependent on. 参数之间的相关性是通过被边缘化的参数建立的。