序言
卡尔曼滤波器（Kalman filter）的名称源于其数学形式与维纳滤波器（Wiener Filter）的渊源，两者均属于最优估计理论的范畴。它们“滤除”的是状态估计中的不确定性（噪声），而非直接处理信号频率。
因此，卡尔曼滤波器更准确地说是一种最优状态估计器，而非传统意义上的频域滤波器。它通过数学模型和概率统计实现动态系统状态的递归估计，其“滤波”本质是对不确定性的递推修正。
卡尔曼滤波的组成
由两部分共 5 个公式组成
预测
更新
卡尔曼滤波的数学推导
数学前提
这里只罗列数学事实，更详见[[多元高斯分布]] 和[[矩阵求导]]
协方差
协方差被用来描述两个随机变量之间线性相关程度，正值就是正相关，负值就是负相关，绝对值越大相关性越高，零就是不相关，
类似于方差 ,协方差 有以下结论 1. 2. 3. 4. 5. 6.
而对于向量 ,我们用一个矩阵描述其各变量间的协方差，即协方差矩阵 ，其对角线上的元素就是各变量的方差，非对角线为两个随机变量之间线性相关程度，显然这是一个对称的矩阵 1. 2. 3. (两随机变量独立时中间项可消)
多元正太分布
正态分布又称高斯分布
类似一元正太分布，其形态完全被均值与方差所描述，多元正太分布的形态完全被均值与协方差所描述，以二元正太分布为例
二元正太分布的形状类似一个小山丘，从俯视图看，则是一层层同心的椭圆“等高线”，均值决定圆心位置，协方差的对角线元素控制椭圆沿 轴和 轴的伸缩程度(长短轴不一定在 轴和 轴上)，而非对角线的元素决定椭圆长轴的倾斜角度(正相关就朝靠近右上方向，负相关就右下)
迹与对迹求导
迹就是矩阵对角线元素之和 1. 2. 3.
对迹求导有 1. 2.
数学模型的建立
我们以一个实例出发，假设有一个小车系统，其某时刻状态 由位置 和速度 组成，即 ，此外，系统还有一个恒定的外力产生的加速度作为控制输入 ，假定两相邻时刻时间差为 ,可知， 即 然而，系统除了控制输入，还有不可控的输入影响，即过程噪声 ,综上，我们就得到了小车系统的状态转移方程
此行的目的，就是为了减少状态估计中的不确定性，为此，我们还至少需要另一种获得 的方法，比如，小车上还有一个相机可以观测到自身的位置与速度(两帧位置差除以两帧时间差)，即 ,其中 是相机观测到真实世界的转换矩阵，如相机给出的实际为像素数，而我们需要将其转换为实际距离，如果相机数据已提前处理好， 有时就是单位矩阵 ， 为测量噪声。
由于噪声不可得，我们先估计噪声不存在，有先验估计 和 (l 另一个先验估计)
现在，我们设存在一个系数 ，可以将 与 融合，得到基于两种方法的最优估计值， 时间紧任务重，我们要马上求出这个关键的系数 ,也就是卡尔曼增益，使得均方误差最小
卡尔曼增益的推导
记 为后验估计误差，其协方差矩阵为 ,记 为先验估计误差，其协方差矩阵为
是一个向量形式的随机变量，我们希望其每一次的均方误差 最小，这与误差协方差的迹最小是等价的
对迹求导 得
更新误差协方差与先验误差协方差的推导
由上面得到的卡尔曼增益，可知， 得
至此，卡尔曼滤波的五个公式已全部给出.
初始化参数的设置
可以预见，理想情况下，卡尔曼增益与误差协方差会最终收敛于一定值，但在实际工程（如目标跟踪、自适应滤波中，由于噪声和模型的变化，它可能不会完全收敛，而是动态调整以保持最优估计。
由那五个公式可知，需要初始化的参数有
其中 不必多说， 如果已知(也可以以第一次观测的数据为准)，相应的， 对角线上的元素(各变量的方差)就可以填入较小的值(如 0) ，而如果 如果未知，则可以根据情况猜一个值并在 对角线上填入较大的值(如 )
非对角线上的元素表征变量间的相关性，可根据实际情况填入，实在不知道就填 0，注意不要超过最大值(两标准差之积)
至于 ，测量噪声可以由传感器的精度给出，过程噪声则要凭实际情况判断
要知道，罗马不是一天建成的，参数不是一次调好的，理论上的最好的参数也不见得就是最好的
事后诸葛亮
卡尔曼滤波作为最优线性无偏估计器，主要依赖于以下假设 1. 线性性，状态转移方程与观测方程均为线性方程； 2. 无偏性，噪声的均值为零，否则会造成预测的偏移(类似控制输入设置错了)； 3. 最小方差，通过对迹求导，可以得到最小方差
而在高斯假设(所有噪声符合高斯分布)下卡尔曼滤波进一步成为全局最优估计器，因为 1. 任意高斯变量在线性映射后仍是高斯分布，在状态转移方程与观测方程均为线性方程的前提下，保证了估计误差同样符合高斯分布； 2. 高斯分布完全由均值和协方差所描述
因此，对卡尔曼滤波的改进方案，主要在两方面 1. 对非线性状态转移方程与观测方程的处理； 2. 对非高斯分布的噪声的处理
比如，扩展卡尔曼(EKF)，通过对非线性方程求一阶导，用一阶微分方程近似原方程的方法，改进了卡尔曼在线性性的局限性。
队内代码
队内代码基于扩展卡尔曼滤波（EKF）实现了装甲板跟踪器的基本功能
状态向量 包含 9个变量，描述装甲板的运动状态 其中 表示装甲板旋转中心的坐标(而非装甲板的中心 )， 表示旋转中心到装甲板的中心的距离， 就表示其绕 轴旋转的角度
状态转移方程
1
2
3
4
5
6
7
8
9
10
11
12
| auto f = [this](const Eigen::VectorXd & x) {
Eigen::VectorXd x_new = x;
x_new(0) += x(1) * dt_;
x_new(2) += x(3) * dt_;
x_new(4) += x(5) * dt_;
x_new(6) += x(7) * dt_
x_new(8) = x(8);
return x_new;
};
|
可见这是一个线性的状态转移方程
观测方程()
1
2
3
4
5
6
7
8
9
| auto h = [](const Eigen::VectorXd & x) {
Eigen::VectorXd z(4);
double xc = x(0), yc = x(2), yaw = x(6), r = x(9);
z(0) = xc - r * cos(yaw);
z(1) = yc - r * sin(yaw);
z(2) = x(4);
z(3) = x(6);
return z;
};
|
因为观测到的是装甲板中心的三维坐标，而
,涉及三角函数，可知观测方程不是线性的，需要用雅可比矩阵做线性近似
不同于上面的实例，这是一个跟踪器，实际敌方装甲板的运动状态未知且多变，我们只好用一个匀速模型近似其在三维空间中的运动，用匀加速模型近似其小陀螺的旋转运动，这导致过程噪声的动态变化，比如突然的转向导致过程噪声的突然增大，同样的，当观测器(相机)离被跟踪的装甲板较远时，观测噪声也会较大，故观测噪声也处在动态的变化中
调整的依据是预测误差(,调整 Q)和测量误差(,调整 R)，我们称两者为创新向量(真不是一个好名字) ，实际过程就是将每一次的误差作为噪声的采样，用以估计噪声的协方差，具体如下
只截取了更新 q_x_x(Q 的第一个元素)的部分
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
| auto u_q = [this](const Eigen::VectorXd & innovation) {
Eigen::MatrixXd q(10, 10);
Eigen::VectorXd innovation_xyz = innovation.segment(0, 6);
double norm_xyz = innovation_xyz.norm();
double adaptive_factor_xyz = std::min(1.0, std::max(0.1, norm_xyz / 10.0))
double x, y;
x = adaptive_factor_xyz * (s2qxyz_max_ - s2qxyz_min_) + s2qxyz_min_;
double t = dt_, r = s2qr_;
double q_x_x = pow(t, 4) / 4 * x, q_x_vx = pow(t, 3) / 2 * x, q_vx_vx = pow(t, 2) * x;
double q_r = pow(t, 4) / 4 * r;
return q;
};
|
可见，当创新向量较大时，过程噪声就会随之调大
以下是 R 的调整( 是偏航角的方差，记为是固定的)
1
2
3
4
5
6
7
| auto u_r = [this](const Eigen::VectorXd & z) {
Eigen::DiagonalMatrix<double, 4> r;
double x = r_xyz_factor;
r.diagonal() << abs(x * z[0]), abs(x * z[1]), abs(x * z[2]), r_yaw;
return r;
};
|