卡尔曼滤波器的推导（Intersection、边缘化概率Marginalization、条件概率Condition）

0. 摘要

这篇博客从高斯分布的角度出发, 比较形象地推导了卡尔曼滤波器; 文章首先从一维高斯分布出发, 然后是二维高斯分布, 先对高斯分布有个大体的了解; 然后使用了一些数学手段来得出高斯的联合概率分布由连部分组成: 条件概率和边缘概率；然后根据这些性质来主动地推出卡尔曼滤波器。
0.1. 卡尔曼滤波器的推导主要分为两步：预测和更新；
卡尔曼滤波器对应到高斯概率分布角度也分为两步：Intersection和Marginalization；所以我们的出发点就是根据高斯分布这两个性质来得到卡尔曼滤波器；
0.2. 推导流程的主体是参照了Michael Andrew Shelley的硕士文章以及《概率机器人》中 3.2节卡尔曼滤波器的推导，自己感觉从这个角度来得到卡尔曼滤波器比较能表述清楚问题。

1. 高斯分布

​ 高斯分布也称作正态分布；$\mathbf{X}\sim \mathcal{N}(\mu ,{\sigma }^2)$表示一个随机变量X服从期望值为$\mu$，方差为${\sigma }^2$的正态分布。

1.1. 一元高斯分布

​其概率密度函数（probability density function）为：
$$p(x;\mu ,\sigma )=\underset{\text{normalization term }}{\underbrace{\frac{1}{\sqrt{2\pi {\sigma }^2}}}}\exp \left(-\frac{(x-\mu {)}^2}{2{\sigma }^2}\right)$$
​需要注意的是：概率密度函数不同于概率，概率不能大于1，但是pdf可以大于1，需要保证概率密度函数曲线积分出来的面积是1；一元正态分布的pdf如下：

图1 一维标准正态分布

%一维高斯函数 a ~ N(mu, sigma)
mu = 0;
sigma = 1;
x = -6:0.1:6;
y = normpdf(x,mu,sigma);
plot(x,y);grid on;
xlabel('x');ylabel('pdf');
legend('标准正态分布');

1.2. 多元高斯分布

​ 其概率密度函数为：
$$\begin{array}{rl}{p}_n(\mathbf{x};\mu ,\mathbf{\Sigma })& =\frac{1}{\sqrt{(2\pi {)}^n|\mathbf{\Sigma }|}}\exp \left(-\frac{1}{2}(\mathbf{x}-\mathbf{\mu }{)}^T{\mathbf{\Sigma }}^{-1}(\mathbf{x}-\mathbf{\mu })\right)\\ & \text{ where }|\mathbf{\Sigma }|\text{ is the determinant of }\mathbf{\Sigma }\end{array}$$
​ 二元正态分布的pdf如下，对应的协方差矩阵分别为：
$$\Sigma =\left[\begin{array}{ll}1& 0\\ 0& 1\end{array}\right];\Sigma =\left[\begin{array}{cc}1& 0.3\\ 0.3& 1\end{array}\right];\Sigma =\left[\begin{array}{cc}1& 0.9\\ 0.9& 1\end{array}\right]$$

图2 多元高斯分布

%二维或多维高斯函数
mu = [-7 0];
sigma = [1 0;0 1];
[x y] = meshgrid(linspace(-10,10,100)', linspace(-10,10,100)');
X = [x(:) y(:)];
z = mvnpdf(X, mu, sigma);
surf(x, y, reshape(z, 100, 100));
xlabel('x');ylabel('y');zlabel('z');
hold on;
%%
mu = [-1 0];
sigma = [1 0.3; 0.3 1];
[x y] = meshgrid(linspace(-10,10,100)', linspace(-10,10,100)');
X = [x(:) y(:)];
z = mvnpdf(X, mu, sigma);
surf(x, y, reshape(z, 100, 100));
%%
mu = [5 0];
sigma = [1 0.9; 0.9 1];
[x y] = meshgrid(linspace(-10,10,100)', linspace(-10,10,100)');
X = [x(:) y(:)];
z = mvnpdf(X, mu, sigma);
surf(x, y, reshape(z, 100, 100));

总结：看完基本的高斯分布，下来就可以看高斯分布的性质了。

1.3. 舒尔补Schur

1.3.1. 首先介绍一下舒尔补Schur的概念，借助Schur可以方便地将矩阵M拆成3个简单矩阵相乘：
$$\mathbf{M}=\left[\begin{array}{ll}\mathbf{A}& \mathbf{B}\\ \mathbf{C}& \mathbf{D}\end{array}\right]=\left[\begin{array}{cc}\mathbf{I}& 0\\ {\mathbf{CA}}^{-1}& \mathbf{I}\end{array}\right]\left[\begin{array}{cc}\mathbf{A}& 0\\ 0& {\Delta }_{\mathbf{A}}\end{array}\right]\left[\begin{array}{cc}\mathbf{I}& {\mathbf{A}}^{-1}\mathbf{B}\\ 0& \mathbf{I}\end{array}\right]$$
​ 其中${\Delta }_{\mathbf{A}}=\mathbf{D}-{\mathbf{CA}}^{-1}\mathbf{B}$称为A关于M的舒尔补。
1.3.2. 舒尔补也可以用来表示一个矩阵的逆：
$${\left[\begin{array}{cc}\mathbf{A}& \mathbf{B}\\ \mathbf{C}& \mathbf{D}\end{array}\right]}^{-1}=\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{A}}^{-1}\mathbf{B}\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{cc}{\mathbf{A}}^{-1}& 0\\ 0& {\Delta }_{\mathrm{A}}^{-1}\end{array}\right]\left[\begin{array}{cc}\mathbf{I}& 0\\ -{\mathbf{CA}}^{-1}& \mathbf{I}\end{array}\right]$$

1.4. 条件概率和边缘概率

1.4.1. 舒尔补可以应用在多元高斯分布上，假设多元变量$\mathbf{X}=\left[\begin{array}{l}a\\ b\end{array}\right]$服从联合高斯分布，设$\mathbf{X}$的均值向量为$\mu =\left(\begin{array}{c}{\mu }_a\\ {\mu }_b\end{array}\right)$，协方差矩阵为 $\Sigma =\left(\begin{array}{cc}A& C^T\\ C& B\end{array}\right)$。我们感兴趣的就是得到条件概率$p(a|b)$和边际概率$p(b)$；

1.4.2. 我们利用舒尔补来得到协方差矩阵的逆（也可以称为高斯过程里的“去相关”）：
$${\left[\begin{array}{cc}A& C^T\\ C& B\end{array}\right]}^{-1}=\left[\begin{array}{cc}I& 0\\ -B^{-1}C& I\end{array}\right]\left[\begin{array}{cc}{\left(A-C^T{B}^{-1}C\right)}^{-1}& 0\\ 0& B^{-1}\end{array}\right]\left[\begin{array}{cc}I& -C^T{B}^{-1}\\ 0& I\end{array}\right]$$
则$p_{a,b}(a,b)$的概率密度为：
$$\begin{array}{rl}{p}_{a,b}(a,b)& =\frac{1}{(2\pi {)}^{n/2}(det({\Sigma }_{\mathbf{X}}){)}^{1/2}}\times \exp \left(-\frac{1}{2}{\left(\begin{array}{l}a-{\mu }_a\\ b-{\mu }_b\end{array}\right)}^T{\left(\begin{array}{ll}A& C^{\top }\\ C& B\end{array}\right)}^{-1}\left(\begin{array}{l}a-{\mu }_a\\ b-{\mu }_b\end{array}\right)\right)\\ & =\frac{1}{(2\pi {)}^{n/2}(det({\Sigma }_{\mathbf{X}}){)}^{1/2}}\times \exp \left(-\frac{1}{2}{\left(\begin{array}{c}a-{\tilde{\mu }}_a\\ b-{\mu }_b\end{array}\right)}^T\left(\begin{array}{cc}{\tilde{A}}^{-1}& 0\\ 0& B^{-1}\end{array}\right)\left(\begin{array}{c}a-{\tilde{\mu }}_a\\ b-{\mu }_b\end{array}\right)\right)\\ & =\sim \times \exp \left(-\frac{1}{2}\left((a-{\tilde{\mu }}_a{)}^T{\tilde{A}}^{-1}(a-{\tilde{\mu }}_a)+(b-{\mu }_b{)}^T{B}^{-1}(b-{\mu }_b)\right)\right)\\ & =\underset{\text{p(a|b)}}{\underbrace{\frac{1}{(2\pi {)}^{n_1/2}(det(\tilde{A}){)}^{1/2}}\exp \left(-\frac{1}{2}(a-{\tilde{\mu }}_a{)}^T{\tilde{A}}^{-1}(a-{\tilde{\mu }}_a)\right)}}\times \underset{\text{p(b)}}{\underbrace{\frac{1}{(2\pi {)}^{n_2/2}(det(B){)}^{1/2}}\exp \left(-\frac{1}{2}(b-{\mu }_b{)}^T{B}^{-1}(b-{\mu }_b)\right)}}\end{array}$$

记：${\tilde{\mu }}_a={\mu }_a+C^T{B}^{-1}(b-{\mu }_b)$，$\tilde{A}=A-C^T{B}^{-1}C$

1.4.3. 从上一节可以得到条件概率和边缘概率：

条件概率: $p(a|b)=\frac{p(a,b)}{p(b)}$
$$a|b\sim \mathcal{N}({\mu }_a+\underset{\text{mean offset }}{\underbrace{C^T{B}^{-1}(b-{\mu }_b)}},A-\underset{\text{couriance offset}}{\underbrace{C^T{B}^{-1}C}})$$
边缘概率: p(b)
$$b\sim \mathcal{N}({\mu }_b,B)$$
Intersection:

​ 如果我们知道x的分布$x\sim \mathcal{N}({\mu }_x,{\Sigma }_{xx})$，y是x的线性组合：$y=Ax+b$，其中A为常数，$b\sim \mathcal{N}(0,\mathcal{Q})$;

​ 那么我们可以得到(x, y)的联合概率密度为：
$$\begin{array}{rl}p\left(\left[\begin{array}{c}x\\ y\end{array}\right]\right)& =\mathcal{N}\left(\left[\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right],\left[\begin{array}{cc}{\Sigma }_{xx}& {\Sigma }_{xy}\\ {\Sigma }_{yx}& {\Sigma }_{yy}\end{array}\right]\right)\\ & =\mathcal{N}\left(\left[\begin{array}{c}{\mu }_x\\ A{\mu }_x+b\end{array}\right],\left[\begin{array}{cc}{\Sigma }_{xx}& {\Sigma }_{xx}{A}^T\\ A{\Sigma }_{xx}& A{\Sigma }_{xx}{A}^T+Q\end{array}\right]\right)\end{array}$$

2.卡尔曼滤波推导

2.1. 运动模型

2.1.1. 想象一个例子，假如一个小车在水平路面上行走，受到某个力的作用，我们要对小车的状态进行估计（比如位置，速度），那么我们可以得到这个小车线性系统的运动模型（模型里的u是t还是t-1? 我这里和《概率机器人》保持了一致）：
$$\begin{array}{rl}\mathbf{x}& \sim \mathcal{N}\left({\mathbf{\mu }}_t,{\mathbf{\Sigma }}_t\right)\\ {\mathbf{x}}_t& ={\mathbf{A}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t+{\mathbf{ϵ}}_t\end{array}$$
​ 其中$ϵ\sim \mathcal{N}(0,\mathcal{Q})$表示运动模型中固有的高斯噪声（因为模型不可能是完全正确无误的），这个噪声是个很小的未知值，会增加状态估计的不确定性；

​ 这里的$\mathbf{x}$表示的是要估计的状态变量，A是状态转移矩阵，表示将$t-1$时刻的状态转移到下一时刻 $t$ ；$\mathbf{u}$是系统的输入（比如发送给机器人的移动指令）并且与状态${\mathbf{x}}_{t-1}$是独立无关，通过矩阵B与状态x建立了线性关系；

2.1.2. 接下来我们要做的就是通过这个线性系统来推出${\mathbf{x}}_t$的概率分布$p({\mathbf{x}}_t)$，这个过程在卡尔曼滤波器中叫作Propagate；我们先可以通过Intersection性质推出${\mathbf{x}}_t$和${\mathbf{x}}_{t-1}$的联合概率密度$p({\mathbf{x}}_t,{\mathbf{x}}_{t-1})$，然后再把${\mathbf{x}}_{t-1}$ Marginalization掉，所以：
$$p\left({\mathbf{x}}_{t-1},{\mathbf{x}}_t\right)=\mathcal{N}\left(\left[\begin{array}{c}{\mu }_{t-1}\\ {\mathbf{A}}_t{\mu }_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t\end{array}\right],\left[\begin{array}{cc}{\Sigma }_{t-1}& {\Sigma }_{t-1}{\mathbf{A}}_t^T\\ {\mathbf{A}}_t{\Sigma }_{t-1}^T& {\mathbf{A}}_t{\mathbf{\Sigma }}_{t-1}{\mathbf{A}}_t^T+{\mathbf{Q}}_t\end{array}\right]\right)$$
​ 把${\mathbf{x}}_{t-1}$ Marginalization掉，将得到${\mathbf{x}}_t$的概率分布（我们这里的对象是预测值${\mathbf{x}}_t$）：
$$\begin{array}{l}{\mathbf{x}}_t\sim \mathcal{N}\left({\mathbf{A}}_t{\mathbf{\mu }}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t,{\mathbf{A}}_t{\mathbf{\Sigma }}_{t-1}{\mathbf{A}}_t^T+{\mathbf{Q}}_t\right)\\ {\mathbf{x}}_t\sim \mathcal{N}\left({\mathbf{\mu }}_t,{\mathbf{\Sigma }}_t\right)\end{array}$$
2.1.3. 通过Propagate我们可以得到 $t$ 时刻 ${\mathbf{x}}_t$ 的协方差：${\mathbf{A}}_t{\mathbf{\Sigma }}_{t-1}{\mathbf{A}}_t^T+{\mathbf{Q}}_t$，可以发现由于状态转移矩阵A和运动模型误差$\mathcal{Q}$的存在增加了系统的不确定性（如一开始的图所示：下一个时刻的高斯分布变得矮胖）；随着时间的推移，如果只使用运动模型来估计系统状态x的输出，结果将越来越不可信。

2.1.4. 为了降低这种不确定性，只有运动模型是不够的，还需要引入测量值measure，这样就涉及到了测量模型，下来我们看一下，系统是如何通过测量值来降低由于 运动方程增加的不确定性 的。

2.2. 测量模型

2.2.1. 这一步我们要做的就是通过测量值$\mathbf{z}$来更新$\mathbf{x}$的概率分布，相当于向这个系统添加一个反馈信息，所以我们得到测量模型（也是一个线性模型）如下：
$$\begin{gathered} \delta \sim \mathcal{N}(0,\mathbf{R}) \\ {\mathbf{z}}_t={\mathbf{C}}_t{\mathbf{x}}_t+{\delta }_t \end{gathered}$$
由于在实际情况中，测量值 $\mathbf{z}$ 往往并不会和待估计的状态 $\mathbf{x}$ 的属性一样，所以还需要一个转换矩阵 $\mathbf{C}$ 来表示两者之间的关系，比如量纲、状态 $\mathbf{x}$ 中的某一个信息是否可以被传感器直接测量或间接测量到等等情形。

其中$\delta$表示测量噪声，服从均值为$0$，协方差为$\mathbf{R}$的高斯分布，比如下图所示，即使陀螺仪处于静止状态，也会受到高斯噪声的影响：

图3 iPhone 4S 处于静止状态下陀螺仪的输出

2.2.2. 下来我们根据上面的线性测量模型通过Intersection得到${\mathbf{x}}_t,{\mathbf{z}}_t$的联合概率分布$p({\mathbf{x}}_t,{\mathbf{z}}_t)$：
$$p\left({\mathbf{x}}_t,{\mathbf{z}}_t\right)=\mathcal{N}\left(\left[\begin{array}{c}{\mu }_t\\ {\mathbf{C}}_t{\mu }_t\end{array}\right],\left[\begin{array}{cc}{\Sigma }_t& {\Sigma }_t{\mathbf{C}}_t^T\\ {\mathbf{C}}_t{\Sigma }_t^T& {\mathbf{C}}_t{\Sigma }_t{\mathbf{C}}_t^T+{\mathbf{R}}_t\end{array}\right]\right)$$
2.2.3. 现在我们得到了两个高斯分布：一个是运动模型得到的预测值高斯分布，另一个是测量值的高斯分布；

2.2.4. 现在我们使用高斯分布的条件概率来给出最优估计的高斯分布（也就是得到两个高斯分布交叉的区域）：

已知：假设多元变量$\mathbf{X}=\left[\begin{array}{l}a\\ b\end{array}\right]$服从联合高斯分布，设$\mathbf{X}$的均值向量为$\mu =\left(\begin{array}{c}{\mu }_a\\ {\mu }_b\end{array}\right)$，协方差矩阵为 $\Sigma =\left(\begin{array}{cc}A& C^T\\ C& B\end{array}\right)$，得到条件概率$p(a|b)$：
$$a|b\sim \mathcal{N}({\mu }_a+\underset{\text{mean offset }}{\underbrace{C^T{B}^{-1}(b-{\mu }_b)}},A-\underset{\text{couriance offset}}{\underbrace{C^T{B}^{-1}C}})$$

于是我们对2.2.2通过条件概率得到$p({\mathbf{x}}_t|{\mathbf{z}}_t)$ （我们这里的对象是最优估计值${\tilde{\mathbf{x}}}_t$, 这个条件概率可以对应于贝叶斯估计的后验概率, 这一步也就是要最大化后验概率MAP）：
$$\begin{array}{rl}{{\mathbf{x}}_t|}_{z=z_t}& \sim \mathcal{N}\left({\mu }_t+{\mathbf{K}}_t\left({\mathbf{z}}_t-{\mathbf{C}}_t{\mathbf{\mu }}_t\right),{\mathbf{\Sigma }}_t-{\mathbf{K}}_t\mathbf{C}{\mathbf{\Sigma }}_t\right)\\ {\mathbf{K}}_t& ={\mathbf{\Sigma }}_t{\mathbf{C}}_t^T{\left({\mathbf{C}}_t{\mathbf{\Sigma }}_t{\mathbf{C}}_t^T+{\mathbf{R}}_t\right)}^{-1}\end{array}$$
其中 ${\mathbf{K}}_t$ 是用来调节模型预测值和测量值之间的权重系数，并且将测量向量 $\mathbf{z}$ 的属性调整为待估计状态向量$\mathbf{x}$ 的属性。

视觉SLAM十四讲中关于最大似然概率的理解MLE:
直观地说，似然是指“在现在的位姿下，可能产生怎样的观测数据”。由于我们知道观测数据，所以最大似然估计，可以理解成：“在什么样的状态下，最可能产生现在观测到的数据” 。这就是最大似然估计的直观意义。

2.3. 总结

​ 由于均值$\mu$处的概率是最大的，下面我们使用它来表示状态值$\mathbf{x}$ ；

2.3.1. 由运动模型得到状态预测值 $\hat{}$ 的均值和协方差，对应于卡尔曼滤波的预测阶段Prediction：

系统运动方程为：${\mathbf{x}}_t={\mathbf{A}}_t{\mathbf{x}}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t+{\mathbf{ϵ}}_t,\text{ where }{\mathbf{ϵ}}_t\sim \mathcal{N}\left(0,{\mathbf{Q}}_t\right)$

• 模型预测值的均值： ${\hat{\mu }}_t={\mathbf{A}}_t{\tilde{\mu }}_{t-1}+{\mathbf{B}}_t{\mathbf{u}}_t$
• 模型预测值的协方差： ${\hat{\mathbf{\Sigma }}}_t={\mathbf{A}}_t{\tilde{\mathbf{\Sigma }}}_{t-1}{\mathbf{A}}_t^T+{\mathbf{Q}}_t$

2.3.2. 由测量模型得到最优输出值$\sim$的均值和协方差，对应于卡尔曼滤波的更新阶段Updated：

系统测量方程为：${\mathbf{z}}_t={\mathbf{C}}_t{\mathbf{x}}_t+{\delta }_t,\text{ where }{\delta }_t\sim \mathcal{N}\left(0,{\mathbf{R}}_t\right)$

• 权重系数： ${\mathbf{K}}_t={\hat{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^T{\left({\mathbf{C}}_t{\hat{\mathbf{\Sigma }}}_t{\mathbf{C}}_t^T+{\mathbf{R}}_t\right)}^{-1}$
• 最优输出值的均值： ${\tilde{\mu }}_t={\hat{\mu }}_t+{\mathbf{K}}_t\left({\mathbf{z}}_t-{\mathbf{C}}_t{\hat{\mu }}_t\right)$
• 最优输出值的协方差： ${\tilde{\mathbf{\Sigma }}}_t={\hat{\mathbf{\Sigma }}}_t-{\mathbf{K}}_t{\mathbf{C}}_t{\hat{\mathbf{\Sigma }}}_t$

3. 参考资料

[1] https://www.cnblogs.com/DemonHunter/p/10740124.html
[2] Master’s thesis of Michael Andrew Shelley: Monocular Visual Inertial Odometry on a Mobile Device
[3] How a Kalman filter works, in pictures
[4] 频谱和功率谱的区别

4. MATLAB常用噪声函数

4.1. rand: Uniformly distributed random numbers, 是0-1的均匀随机分布的噪声(FFT结果会集中在最左侧);

4.2. randn: 是均值为0方差为1的标准正态分布的高斯白噪声(FFT结果是每个频率处都有)。

4.3. wgn: Generate white Gaussian noise, 传入的参数dBW表示的是var的大小.

$$var(data)=10^{\frac{dBW}{10}}$$

高斯白噪声中的高斯是指概率分布是正态函数.
高斯白噪声的功率谱密度服从均匀分布，幅度分布服从高斯分布(也就是说高斯白噪声的频谱是布满整个频域的), 其功率谱密度频谱图和噪声幅值分布图的图片如下：

白噪声（white noise）是指功率谱密度在整个频域内是常数的噪声。
白噪声或白杂讯，是一种功率谱密度为常数的随机信号。换句话说，此信号在各个频段上的功率谱密度是一样的，由于白光是由各种频率（颜色）的单色光混合而成，因而此信号的这种具有平坦功率谱的性质被称作是“白色的”，此信号也因此被称作白噪声。

4.4. 代码如下:

clear
clc
close all
%% Create rand data
dotNumber = 102400;
N = 1:dotNumber;
data_rand = rand([1, dotNumber]);
var_data_rand = var(data_rand);
mean_data_rand = mean(data_rand);

figure(1)
subplot(3,1,1)
plot(N, data_rand);title('data rand');
subplot(3,1,2)
histogram(data_rand);title('Amplitude Distribution');
subplot(3,1,3)
plot(N, abs(fft(data_rand)));title('Spectrum');

%% Create randn data
data_randn = randn([1, dotNumber]);
var_data_randn = var(data_randn);
mean_data_randn = mean(data_randn);

figure(2)
subplot(3,1,1)
plot(N, data_randn);title('data randn');
subplot(3,1,2)
histogram(data_randn);title('Amplitude Distribution');
subplot(3,1,3)
plot(N, abs(fft(data_randn)));title('Spectrum');

%% Create wgn data
data_wgn = wgn(1, dotNumber, -10);
var_data_wgn = var(data_wgn);
mean_data_wgn = mean(data_wgn);

figure(3)
subplot(3,1,1)
plot(N, data_wgn);title('data wgn');
subplot(3,1,2)
histogram(data_wgn);title('Amplitude Distribution');
subplot(3,1,3)
plot(N, abs(fft(data_wgn)));title('Spectrum');