卡尔曼滤波器（KF线性滤波器）
Learn From DR_CAN And 无比机智的永哥 Mainly
Write By Champrin From 2022-10-12 To 2022-10-24
GUET Evolution Team Visual Group
目录
卡尔曼滤波器是最优化递归数字处理算法，它能对存在不确定性的系统进行其状态预测
系统的不确定性
- 不存在完美的数学模型
- 系统的扰动不可控，也很难建模
- 测量传感器存在误差
数学基础
- 线性代数
- 概率论及数理统计
- 方差
- 协方差
- 正态（高斯）分布
- 数据融合
状态空间方程及观测器基础
- 现代控制理论
- 自动控制原理
- 模型预测控制
- 系统动力学
如果想要自己对某个系统的状态进行建模，尤其是复杂的系统，应该是需要上面那些东西
I.递归算法
例子：获得某一物体长度的真实值
设测量结果：\(Z_k\)，\(k\)表示第k次的测量结果
使用测量仪器进行测量，因为存在测量误差，测量的结果不是真实值
为了估计真实数据，取平均值是个好方法
设有\(k\)次测量结果，估计真实数据为 \(\hat{x}_k\)
\[\begin{aligned} \hat{x}_k & = \frac{1}{k} (z_1 + z_2 + \cdots + z_k)\\ & = \frac{1}{k} (z_1 + z_2 + \cdots + z_{k-1}) + \frac{1}{k}z_k\\ & = \frac{1}{k} \frac{k-1}{k-1} (z_1 + z_2 + \cdots + z_{k-1}) + \frac{1}{k}z_k\\ & = \frac{k-1}{k} \frac{1}{k-1} (z_1 + z_2 + \cdots + z_{k-1}) + \frac{1}{k}z_k\\ & = \frac{k-1}{k} \hat{x}_{k-1} + \frac{1}{k}z_k\\ & = (1 - \frac{1}{k}) \hat{x}_{k-1} + \frac{1}{k}z_k\\ & = \hat{x}_{k-1} - \frac{1}{k} \hat{x}_{k-1} + \frac{1}{k}z_k\\ \end{aligned} \]\[\Rightarrow \hat{x}_k = \hat{x}_{k-1} + \frac{1}{k}(z_k - \hat{x}_{k-1} ) \]在往后中，遇到变量头上有个“帽子”，如\(\hat{X}、\hat{Y}、\hat{Z}\)代表这个变量为估计值
其中，把 \(\frac{1}{k}\) 设为 \({\bf K_k}\) ，称为卡尔曼增益/因数：Kalman Gain
\[\Rightarrow \hat{x}_k = \hat{x}_{k-1} + {\bf K_k}(z_k - \hat{x}_{k-1}) \tag{1.1} \]显然
- 若 \(k \uparrow\) ， \(\frac{1}{k} \rightarrow 0\) ，\(K_k \rightarrow 0\) ， 此时 \(\hat{x}_k \rightarrow \hat{x}_{k-1}\)
随着 \(k \uparrow\) ， \(Z_k\) 测量结果不再重要，也就是当有了大量的数据，即测量很多次以后，对估计的结果很有信心，以后的测量值变得不那么重要- 若 \(k\) 小，即 \(\frac{1}{k}\) 大， \(Z_k\) 的作用就大，尤其是在与估计值差距比较大的时候
\((1.1)\) 也可以表示为
\[当前估计值 = 上一次的估计值 + 系数 * (当前测量值 - 上一次估计值) \]这就是递归思想
再进一步对卡尔曼增益\(K_k\)进行处理
\(K_k\) 具体推导会在后面中讲到，这里先暂时用着
设：
估计误差(估计值和真实值的差距)： \(e_{EST_{k}}\)
测量误差(测量值和真实值的差距)： \(e_{MEA_{k}}\)
这里的 \(K_k\) 与一般形式的卡尔曼增益中的变量不同，但是整体形式一样，看到后面就明白了
在\(k\)时刻，
- 若 \(e_{EST_{k-1}} \gg e_{MEA_{k}}\)
\(K_k \rightarrow 1 \quad \hat{x}_k = \hat{x}_{k-1} + z_k - \hat{x}_{k-1} = z_k\)
估计误差大，那么测量的更加准确，就要更信任测量值- 若 \(e_{EST_{k-1}} \ll e_{MEA_{k}}\)
\(K_k \rightarrow 0 \quad \hat{x}_k = \hat{x}_{k-1}\)
测量误差大，就要更信任估计值
对于上一个例子的应用：
- 计算 \(K_k = \frac{e_{EST_{k-1}}}{e_{EST_{k-1}}+e_{MEA_{k}}}\)
- 计算 \(\hat{x}_k = \hat{x}_{k-1} + {K_k}(z_k - \hat{x}_{k-1})\)
- 更新 \(e_{EST_k} = (1-K_k)e_{EST_{k-1}}\)
对于 3.更新 \(e_{EST_k}\) 这条式子与后面的更新估计误差协方差 \(P_k\) 的整体形式一样，看到后面有关 \(P_k\) 的推导也一样的会明白这条式子是怎么来的了
假设，一个物体的实际长度是 \(50mm\) (真实值)
但是，实际我们是不知道这个真实值的
先用递归算法估计这个真实值
随便估计 \(\hat{x}_0 = 40mm\) ，随便给出 \(e_{EST_0} = 5mm\)
设我们所用的测量工具，如尺子的测量误差为 \(3mm\) ，即 \(e_{MEA} = 3mm\)
- 测得第一次，即 \(k = 1\) 的测量结果 \(z_1 = 51mm\)
- 计算 \(K_1 = \frac{e_{EST_0}}{e_{EST_0}+e_{MEA_1}} = \frac{5}{5+3} = 0.625\)
- 计算 \(\hat{x}_1 = \hat{x}_0 + {K_1}(z_1 - \hat{x}_0) = 40 + 0.625*(51 - 40) = 46.875\)
- 更新 \(e_{EST_1} = (1-K_1)e_{EST_0} = (1-0.625)*5 = 1.875\)
- 测得第二次的测量结果 \(z_2\) 重复以上步骤
- 测得第三次的测量结果 \(z_3\) 重复以上步骤
... - 测得第\(k\)次的测量结果 \(z_k\) 重复以上步骤
绘制成表格，以及曲线图：
因为测量仪器的测量误差为3mm，那么每次的测量结果应该在 \([50-3,50+3]\) 即 \([47,53]这个区间范围内\)
肉眼可见的，随着测量次数 \(k\) 不断增加，估计值逐渐趋向于实际真实值\(50\)
也就是估计值到最后，能够估计出一个无限靠近实际真实值的一个值来
尽管测量值一直在跳变
II.数学基础
涉及：数据融合 协方差矩阵 状态空间方程及观测器 先验估计
i.数据融合
例子：有两个秤，秤同一个物品，如何估计它的真实重量
它们分别称出的重量为 \(Z_1 = 30g\) ， \(Z_2 = 32g\)
且满足正态分布，标准差分别为 \(\sigma_1 = 2g\) ， \(\sigma_2 = 4g\)
那么怎么样估计它的真实重量值 \(\hat{Z}\) 呢
就要用到上一节所说的递归算法
\(k \in [0,1]\)
- \(k = 0,\hat{Z} = Z_1\)
- \(k = 1,\hat{Z} = Z_2\)
为了估计真实值，我们的目标是：求k，使得 \(\sigma_{\hat{Z}}\) 最小，也即方差 \(var(\hat{Z})\) 最小
\[\begin{aligned} \sigma^2_{\hat{Z}} & = var(Z_1 + k(Z_2 - Z_1)) \\ & = var((1-k)Z_1 + kZ_2) & \text{$(1-k)Z_1和kZ_2是相互独立的$}\\ & = var((1-k)Z_1) + var(kZ_2) \\ & = (1-k)^2var(Z_1) + k^2var(Z_2) \\ & = (1-k)^2\sigma^2_1 + k^2\sigma^2_2 \\ \end{aligned} \]标准差可以理解为不确定度
追求不确定性最小，即可把握最大的预测
求解最小值常用求导法
\[\begin{aligned} \frac{d\sigma^2_{\hat{Z}}}{dk} = 0 \\ \Rightarrow -2(1-k)\sigma^2_1 + 2k\sigma^2_2 & = 0 \\ \Rightarrow k = \frac{\sigma^2_1}{\sigma^2_1+\sigma^2_2} \\ \end{aligned} \]可以解得 \(k=0.2\)
因此 \(\hat{Z} = Z_1 + k(Z_2 - Z_1) = 30 + 0.2 * (32 - 30) = 30.4\)
至此，我们通过数学证明，这是最优解
同时 \(\sigma^2_{\hat{Z}} = (1-k)^2\sigma^2_1 + k^2\sigma^2_2 = (1-0.2)^2*2^2 + (0.2)^2*4^2=3.2\)
我们可以把这三个正态分布表示出
蓝色为数据融合之后的正态分布，直观的看出这的确是最优解，它融合了两个值
ii.协方差矩阵
方差，协方差在一个矩阵中表现出来，体现变量间的联动关系
例子：求球员的身高、体重、年龄的相关性
| 球员 | 身高(\(x\)) | 体重(\(y\)) | 年龄(\(y\)) |
|---|---|---|---|
| 1 | \(179\) | \(74\) | \(33\) |
| 2 | \(187\) | \(80\) | \(31\) |
| 3 | \(175\) | \(71\) | \(28\) |
进而可以求得 \(x,y,z\) 的平均值和方差
| 平均值\(E\) | 方差\(\sigma^2\) | ||
|---|---|---|---|
| \(E(x)\) | \(180.3\) | \(\sigma^2_{x}\) | \(24.89\) |
| \(E(y)\) | \(75\) | \(\sigma^2_{y}\) | \(14\) |
| \(E(z)\) | \(30.7\) | \(\sigma^2_{z}\) | \(4.22\) |
再而求出 \(x,y,z\) 的协方差
| 协方差 | |
|---|---|
| \(\sigma_x\sigma_y\) | \(18.7\) |
| \(\sigma_y\sigma_x\) | \(18.7\) |
| \(\sigma_x\sigma_z\) | \(4.4\) |
| \(\sigma_z\sigma_x\) | \(4.4\) |
| \(\sigma_z\sigma_y\) | \(3.3\) |
| \(\sigma_y\sigma_z\) | \(3.3\) |
协方差大于\(0\)，说明两变量的变化方向一致，值越大说明相关性越强
协方差矩阵为
\[P = \begin{bmatrix} \sigma^2_x & \sigma_x\sigma_y & \sigma_x\sigma_z \\\\ \sigma_y\sigma_x & \sigma^2_y & \sigma_y\sigma_z \\\\ \sigma_z\sigma_x & \sigma_z\sigma_y & \sigma^2_z \end{bmatrix} = \begin{bmatrix} 24.89 & 18.7 & 4.4 \\\\ 18.7 & 14 & 3.3 \\\\ 4.4 & 3.3 & 4.22 \end{bmatrix} \]过渡矩阵为
\[a = \begin{bmatrix} x_1 & y_1 & z_1 \\\\ x_2 & y_2 & z_2 \\\\ x_3 & y_3 & z_3 \end{bmatrix} - \frac{1}{3} \begin{bmatrix} 1 & 1 & 1 \\\\1 & 1 & 1 \\\\ 1 & 1 & 1 \end{bmatrix}\begin{bmatrix} x_1 & y_1 & z_1 \\\\ x_2 & y_2 & z_2 \\\\ x_3 & y_3 & z_3 \end{bmatrix} \]\[P = \frac{1}{3}a^Ta \]\(\frac{1}{3} \begin{bmatrix} 1 & 1 & 1 \\\\1 & 1 & 1 \\\\ 1 & 1 & 1 \end{bmatrix}\begin{bmatrix} x_1 & y_1 & z_1 \\\\ x_2 & y_2 & z_2 \\\\ x_3 & y_3 & z_3 \end{bmatrix}\)实际意义为求 \(x,y,z\) 的平均值
iii.状态空间的表达及观测器
在某一状态空间，它的动态方程为： \(m\ddot{x} + B\dot{x} + kx = F\)
\(x\)是我们需要掌握的，它代表位移
但动态方程已经它的各个参数如 \(m,B,k\) 等我们无需知道它的具体含义
\(F\) 为系统输入，用 \(U\) 替代\(F\) ，进而整理为：
\(\ddot{x} = \frac{1}{m}U - \frac{B}{m}\dot{x} - \frac{k}{m}x\)
化成状态空间的表达形式
状态变量：
令\(x_1 = x, x_2 = \dot{x}\)
\(\dot{x}_1 = \dot{x} = x_2 \\ \dot{x}_2 = \ddot{x} = \frac{1}{m}u - \frac{B}{m}\dot{x}-\frac{k}{m}x = \frac{1}{m}u - \frac{B}{m}x_2-\frac{k}{m}x_1\)
测量变量：
测量位置 \(z_1 = x = x_1\)
测量速度 \(z_2 = \dot{x} = x_2\)
转换为矩阵形式，得出状态方程，为状态空间的表达形式
\(\begin{bmatrix}\dot{x}_1 \\ \dot{x}_2\end{bmatrix} = \begin{bmatrix} 0 & 1 \\ -\frac{k}{m} & - \frac{B}{m} \end{bmatrix}\begin{bmatrix}x_1 \\ x_2\end{bmatrix} + \begin{bmatrix}0 \\ \frac{1}{m} \end{bmatrix}u\)
\(\Rightarrow \dot{X}_{(t)} = AX_{(t)} + BU_{(t)}\)
转换为矩阵形式，得出测量方程
\(\begin{bmatrix}z_1 \\ z_2\end{bmatrix} = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix}\begin{bmatrix}x_1 \\ x_2\end{bmatrix}\)
\(\Rightarrow Z_{(t)} = HX_{(t)}\)
以上的两方程为连续的表达形式，随时间的变化
实际问题中，需要离散的表达形式，这怎么由连续的表达方程转为离散的表达方程？TODO
转换为离散的形式
\(X_k = AX_{k-1} + BU_{k-1} \\ Z_k = HX_k\)
下标\(k - 1,k,k + 1,...\)
每一个单位1，代替一个时间单位，即采样时间：Sample Time
连续性转为离散型，并不是直接照抄连续性的矩阵结果，需要根据采样时间来计算
离散型体现了一种变化，从上一步到这一步的一种变化
由于在实际问题中，具有不确定性，现实环境的复杂，即使是线性关系不是完全平滑的，总是会存在一些扰动。
因此，上面的两个方程都应该带有噪声，代表这种不确定性/扰动。
计算结果：
\[X_k = AX_{k-1} + BU_{k-1} + W_{k-1} \tag{2.1} \]测量结果：
\[Z_k = HX_k + V_k \tag{2.2} \]\(W_{k-1}\) 表示过程噪声，一种不确定性
\(V_k\) 表示测量噪声，测量工具的误差
因此，在有噪声，即有不确定性的情况下，我们如何依靠计算结果和测量结果估计真实值？
这就是卡尔曼滤波器所能解决的
III.卡尔曼滤波器 详细推导
在上一节内容中，已经得出：
计算结果：\(X_k = AX_{k-1} + BU_{k-1} + W_{k-1}\)
其中：\(W \sim N(0,Q)\)
\(W\)符合正态分布，期望为\(E(W) = 0\) ， \(Q \in R^{n^2}\) 为过程噪声协方差矩阵， \(Q = E\begin{bmatrix} WW^T \end{bmatrix}\) ，表示W元素之间的相关关系
测量结果：\(Z_k = HX_k + V_k\)
其中：\(V \sim N(0,R)\)
\(V\)符合正态分布，期望为\(E(V) = 0\) ， \(R \in R^{m^2}\) （右侧的R代表实数）为测量噪声协方差矩阵， \(R = E\begin{bmatrix} VV^T \end{bmatrix}\) ，表示V元素之间的相关关系
其中：协方差矩阵 \(Q、R\) 是随着状态变化的，但一般实际问题中，假设是不变的
\(A^T\) 为矩阵 \(A\) 的转置
有关协方差矩阵为什么能写成\(Q = E\begin{bmatrix} WW^T \end{bmatrix}\)这种形式
\(k−1\) 和 \(k\) 分别表示上一状态和当前状态
\(X \in R^n\) ：状态变量矩阵
\(A \in R^{n^2}\) ：状态转移矩阵，从上一状态到当前状态的转换矩阵
\(B \in R^{n*l}\) ：状态输入矩阵（控制矩阵），系统输入到当前状态的转换矩阵
\(U \in R^l\) ：系统输入，一般实际使用中是忽略的
\(W \in R^n\) ：过程噪声矩阵，主要是从上一状态进入到当前状态时，会有许多外界因素的干扰，也是在表示系统的不确定性，实际中状态变量并不一定是按照状态方程来执行的\(Z \in R^m\) ：测量变量矩阵
\(H \in R^{m*n}\) ：状态观察矩阵（传输矩阵），当前状态到测量的转换矩阵
\(V \in R^m\) ：测量噪声矩阵，测量仪器会存在误差
其中： \(A、B、H、W、V\) 是随着状态变化的，但一般实际问题中，假设是不变的
先验估计值 及 后验估计值
但是，\(W_{k-1}\)和\(V_k\)为噪声，无法准确建模
因此，对于这两个方程，除\(W_{k-1}\)和\(V_k\)之外的前部分才是我们自身所能掌握的
在此引出先验估计值，它表示仅仅利用过程先验知识求出的当前状态的先验状态估计(A priori state estimate)，是在不考虑过程噪声的情况下，根据状态方程计算出来的
一般解决实际问题时，没有系统输入 \(U\) ，那么先验估计值为\(\hat{X}^-_k = A\hat{X}_{k-1}\)
这个方程表示算出来的
\(X_k\) 缺少 \(W\) ，只能是估计值 \(\hat{X}\) ，且未做任何处理
\(X_{k-1}\) 也是无法真实得到的，也只能是估计值 \(\hat{X}_{k-1}\)
对于忽略测量噪声的测量值，有两种解释：
- 第一种：
有关 \(H\) 的争议
此处方程不带\(V\)，所以也能是估计值
这个方程表示测出来的
\(\hat{X}^-_k\) 不带有噪声，为理论计算结果，是不太准确的结果；\(Z_k\)存在测量仪器的误差，也是不太准确的结果；同时，\(W、V\)未知，也无法建模。
此情此景，就需要用到上节所说的数据融合，由两个不太准确的结果，得到一个准确的结果
因此：
\(\hat{X}_k\) 表示后验/最终估计值
\(G \in [0,1]\)
- \(G = 0, \hat{X}_k = \hat{X}^-_k\)
- \(G = 1, \hat{X}_k = H^{-1}Z_k\)
将G替换成 \(G = K_kH\) ，可以得到
\[\hat{X}_k = \hat{X}^-_k + K_k(Z_k - H\hat{X}^-_k) \tag{3.3} \]\(K_k \in [0,H^{-1}] , K_k \in R^{n*m} , 卡尔曼增益(gain)/均化系数(blending factor)\)
- \(K_k = 0, \hat{X}_k = \hat{X}^-_k\)
- \(K_k = H^{-1}, \hat{X}_k = H^{-1}Z_k\)
- 第二种
\(Z^-_k\) 表示忽略测量噪声，根据当前先验状态得到的无测量噪声的测量值，也可以表示为预测的测量值：
\[Z^-_k = H\hat{X}^-_k \]\(\dot{Z}_k\) 表示预测的测量值和实际测量值的差异：
\[\dot{Z}_k = Z_k - Z^-_k = Z_k - H\hat{X}^-_k \]表示实际测量值（包含了上面提到的各种干扰即过程噪声以及测量噪声）减去无噪声测量值(一般被叫做measurement innovation或者是residual)
实际得到是过程噪声和测量噪声对当前测量值的影响（当然也会影响当前状态），也即差异，也就是包含了 \(W\) 和 \(V\) 的情况，是一个噪声变量
现在得到了一个无噪声的状态的估计 \(\hat{X}^-_k\) ，以及一个噪声变量 \(\dot{Z}_k\)
根据经验来说把这两个值按照一定的方式整合起来就可以得到后验状态估计，而卡尔曼滤波中采用按照比例组合的方式，这里也有点数据融合的味道
两种解释最终的结果都是一样的
更新卡尔曼增益/均化系数
我们的目标是：寻找 \(K_k\) 使得 \(\hat{X}_k \rightarrow X_k(实际值)\)
它会与 \(W\)，\(V\) 有关，当 \(W\) 大，会相信 \(V\)；当 \(V\) 大，会相信 \(W\)
引入：估计误差\(e_k = X_k - \hat{X}_k\)
其中： \(e_k\sim N(0,P)\)
\(e_k\)符合正态分布，期望为\(0\)，\(P\)为状态估计误差协方差矩阵，\(P = E\begin{bmatrix} ee^T \end{bmatrix}\)
因为希望估计值与真实值的差距越小，即误差的方差越小，则越接近于期望\(0\)
而对于一个矩阵的迹，即迹 \(tr(P) = 对角线相加\)，也就是所有的误差的方差相加
也即为了使后验状态估计 \(\hat{X}_k\) 最优，那么目标就是要最小化估计误差协方差矩阵 \(P_k\) 的值
有关⽅差就是最⼩的理解
那么对于我们的目标，就可以等价为：寻找 \(K_k\) 使得 \(tr(P)\) 最小
下面就开始寻找我们的目标：
\[\begin{aligned} P_k & = E\begin{bmatrix} (X_k - \hat{X}_k){(X_k - \hat{X}_k)}^T \end{bmatrix} \\ & = E\begin{bmatrix} [(I - K_kH)e^-_k - K_kV_k]{[(I - K_kH)e^-_k - K_kV_k]}^T \end{bmatrix} \\ & = E\begin{bmatrix} [(I - K_kH)e^-_k - K_kV_k][{e^-_k}^T(I - K_kH)^T - V_k^TK_k^T] \end{bmatrix} \\ & = E\begin{bmatrix} (I - K_kH)e^-_k{e^-_k}^T(I - K_kH)^T - (I - K_kH)e^-_kV_k^TK_k^T - K_kV_k{e^-_k}^T(I - K_kH)^T + K_kV_kV_k^TK_k^T \end{bmatrix} \\ & = E\begin{bmatrix}(I - K_kH)e^-_k{e^-_k}^T(I - K_kH)^T \end{bmatrix} - E\begin{bmatrix}(I - K_kH)e^-_kV_k^TK_k^T \end{bmatrix} - E\begin{bmatrix}K_kV_k{e^-_k}^T(I - K_kH)^T \end{bmatrix} + E\begin{bmatrix}K_kV_kV_k^TK_k^T \end{bmatrix} \\ \end{aligned} \]这里的 \(Z_k\) 为真实测量值，因为是带入了方程 \((3.3)\hat{X}_k = \hat{X}^-_k + K_k(Z_k - H\hat{X}^-_k)\)
因此要带噪声 \(V_k\) ，而且此处考虑的是噪声对估计误差的影响，自然需要考虑 \(V_k\)
\((AB)^T = B^TA^T\)
\((A+B)^T = A^T + B^T\)
\[\begin{aligned} P_k & = E\begin{bmatrix}(I - K_kH)e^-_k{e^-_k}^T(I - K_kH)^T \end{bmatrix} - E\begin{bmatrix}(I - K_kH)e^-_kV_k^TK_k^T \end{bmatrix} - E\begin{bmatrix}K_kV_k{e^-_k}^T(I - K_kH)^T \end{bmatrix} + E\begin{bmatrix}K_kV_kV_k^TK_k^T \end{bmatrix} \\ & = (I - K_kH)E\begin{bmatrix}e^-_k{e^-_k}^T\end{bmatrix}(I - K_kH)^T - (I - K_kH)E\begin{bmatrix}e^-_kV_k^T\end{bmatrix}K_k^T - K_kE\begin{bmatrix}V_k{e^-_k}^T\end{bmatrix}(I - K_kH)^T + K_kE\begin{bmatrix}V_kV_k^T\end{bmatrix}K_k^T \\ \end{aligned} \]其中：
\(I,K_kH\)为常数
那么：
\[\begin{aligned} P_k & = (I - K_kH)E\begin{bmatrix}e^-_k{e^-_k}^T\end{bmatrix}(I - K_kH)^T - (I - K_kH)E\begin{bmatrix}e^-_kV_k^T\end{bmatrix}K_k^T - K_kE\begin{bmatrix}V_k{e^-_k}^T\end{bmatrix}(I - K_kH)^T + K_kE\begin{bmatrix}V_kV_k^T\end{bmatrix}K_k^T \\ & = (I - K_kH)E\begin{bmatrix}e^-_k{e^-_k}^T\end{bmatrix}(I - K_kH)^T + K_kE\begin{bmatrix}V_kV_k^T\end{bmatrix}K_k^T \\ \end{aligned} \]其中：
当 \(A\) ， \(B\) 相互独立时， \(E(AB) = E(A)E(B)\)
\(E(e_k) = 0\) ， \(E(V_k) = 0\) ，且 \(e_k\) ， \(V_k\) 相互独立 \(\Rightarrow E(e_kV_k) = 0\)
也即 \(E\begin{bmatrix}e^-_kV_k^T\end{bmatrix} = 0\) ， \(E\begin{bmatrix}V_k{e^-_k}^T\end{bmatrix}=0\)
所以： \((I - K_kH)E\begin{bmatrix}e^-_kV_k^T\end{bmatrix}K_k^T = 0\) ， \(K_kE\begin{bmatrix}V_k{e^-_k}^T\end{bmatrix}(I - K_kH)^T=0\)
那么：
\[\begin{aligned} P_k & = (I - K_kH)E\begin{bmatrix}e^-_k{e^-_k}^T\end{bmatrix}(I - K_kH)^T + K_kE\begin{bmatrix}V_kV_k^T\end{bmatrix}K_k^T \\ & = (I - K_kH)P^-_k(I - K_kH)^T + K_kRK_k^T \\ & = (P^-_k - K_kHP^-_k)(I - K_kH)^T + K_kRK_k^T \\ & = (P^-_k - K_kHP^-_k)(I^T - H^TK_k^T) + K_kRK_k^T \\ & = P^-_k - P^-_kH^TK_k^T - K_kHP^-_k + K_kHP^-_kH^TK_k^T + K_kRK_k^T \\ \end{aligned} \]其中：
\(P = E\begin{bmatrix} ee^T \end{bmatrix}\) ， 即 \(E\begin{bmatrix}e^-_k{e^-_k}^T\end{bmatrix} = P^-_k\)
\(R = E\begin{bmatrix} VV^T \end{bmatrix}\) ， 即 \(E\begin{bmatrix} VV^T \end{bmatrix} = R\)
那么：
\[\begin{aligned} (P^-_kH^TK_k^T)^T & = (K_k^T)^T(P^-_kH^T)^T \\ & = K_k(P^-_kH^T)^T \\ & = K_k(H^T)^T(P^-_k)^T \\ & = K_kH(P^-_k)^T \\ & = K_kHP^-_k \\ \end{aligned} \]其中：
\(P_k\) 为协方差矩阵，有 \(P_k = P_k^T\)
那么：
\[\begin{aligned} tr(P_k) & = tr(P^-_k) - tr(P^-_kH^TK_k^T) - tr(K_kHP^-_k) + tr(K_kHP^-_kH^TK_k^T) + tr(K_kRK_k^T) \\ & = tr(P^-_k) - 2tr(K_kHP^-_k) + tr(K_kHP^-_kH^TK_k^T) + tr(K_kRK_k^T) \\ \end{aligned} \]其中：
对于矩阵来说，原矩阵与其矩阵的转置，对角线相加都是相同的，即原矩阵与其矩阵的转置的迹是相同的
即 \(tr(K_kHP^-_k) = tr((P^-_kH^TK_k^T)^T) = tr(P^-_kH^TK_k^T)\)
那么：
回顾我们的目标：寻找 \(K_k\) 使得 \(tr(P)\) 最小
而对于最小值极值类问题，常用求导解决
即求：
\[\begin{aligned} \frac{dtr(P_k)}{dK_k} & = 0 - 2(HP^-_k)^T + 2K_kHP^-_kH^T + 2K_kR \\ & = -2(P^-_k)^TH^T + 2K_k(HP^-_kH^T + R) \\ & = -2P^-_kH^T + 2K_k(HP^-_kH^T + R) \\ \end{aligned} \]\[即 -2P^-_kH^T + 2K_k(HP^-_kH^T + R) = 0 \]\[\Rightarrow -P^-_kH^T + K_k(HP^-_kH^T + R) = 0 \]\[\Rightarrow K_k = \frac{P^-_kH^T}{HP^-_kH^T + R} \tag{3.4} \]其中：
\(\frac{dtr(AB)}{dA} = B^T\)
\(\frac{dtr(ABA^T)}{dA} = 2AB\)
\(\frac{dtr(B)}{dA} = 0, B为常数矩阵\)
那么：
由此，我们推导出了卡尔曼增益/因数：\(K_k\)
\(R\) 是测量噪声误差协方差矩阵
当\(R \uparrow , K_k \rightarrow 0 , \hat{X}_k = \hat{X}^-_k\) 即测量噪声大，相信先验估计的结果，即计算（估计）结果 \(\hat{X}^-_k\)
当\(R \downarrow , K_k \rightarrow H^{-1} , \hat{X}_k = H^{-1}Z_k\) 即测量噪声小，相信测量的结果
更新先验误差协方差矩阵 及 更新误差协方差矩阵
到此，我们还剩下 \(P^-_k\) 以及 \(P_k\) 还未知，接下来我们推导这两个：
\[P^-_k = E\begin{bmatrix}e^-_k{e^-_k}^T\end{bmatrix} \]\[\begin{aligned} 其中:e^-_k & = X_k - \hat{X}^-_k \\ & = AX_{k-1} + BU_{k-1} + W_{k-1} - A\hat{X}_{k-1} - BU_{k-1} \\ & = A(X_{k-1} - \hat{X}_{k-1}) + W_{k-1} \\ & = Ae_{k-1} + W_{k-1} \\ \end{aligned} \]\[\begin{aligned} P^-_k & = E\begin{bmatrix}e^-_k{e^-_k}^T\end{bmatrix} \\ & = E\begin{bmatrix}(Ae_{k-1} + W_{k-1})(Ae_{k-1} + W_{k-1})^T\end{bmatrix} \\ & = E\begin{bmatrix}(Ae_{k-1} + W_{k-1})(e_{k-1}^TA^T + W_{k-1}^T)\end{bmatrix} \\ & = E\begin{bmatrix}Ae_{k-1}e_{k-1}^TA^T + Ae_{k-1}W_{k-1}^T + W_{k-1}e_{k-1}^TA^T + W_{k-1}W_{k-1}^T\end{bmatrix} \\ & = E\begin{bmatrix}Ae_{k-1}e_{k-1}^TA^T\end{bmatrix} + E\begin{bmatrix}Ae_{k-1}W_{k-1}^T\end{bmatrix} + E\begin{bmatrix}W_{k-1}e_{k-1}^TA^T\end{bmatrix} + E\begin{bmatrix}W_{k-1}W_{k-1}^T\end{bmatrix} \\ & = AE\begin{bmatrix}e_{k-1}e_{k-1}^T\end{bmatrix}A^T + AE\begin{bmatrix}e_{k-1}W_{k-1}^T\end{bmatrix} + E\begin{bmatrix}W_{k-1}e_{k-1}^T\end{bmatrix}A^T + E\begin{bmatrix}W_{k-1}W_{k-1}^T\end{bmatrix} \\ \end{aligned} \]\[\begin{aligned} P^-_k & = AE\begin{bmatrix}e_{k-1}e_{k-1}^T\end{bmatrix}A^T + AE\begin{bmatrix}e_{k-1}W_{k-1}^T\end{bmatrix} + E\begin{bmatrix}W_{k-1}e_{k-1}^T\end{bmatrix}A^T + E\begin{bmatrix}W_{k-1}W_{k-1}^T\end{bmatrix} \\ & = AP_{k-1}A^T + Q \end{aligned} \]\[\Rightarrow P^-_k = AP_{k-1}A^T + Q \tag{3.5} \]\[\begin{aligned} P_k & = P^-_k - P^-_kH^TK_k^T - K_kHP^-_k + K_kHP^-_kH^TK_k^T + K_kRK_k^T \\ & = P^-_k - P^-_kH^TK_k^T - K_kHP^-_k + K_k(HP^-_kH^T + R)K_k^T \\ \end{aligned} \]\[\begin{aligned} 其中：K_k(HP^-_kH^T + R)K_k^T & = \frac{P^-_kH^T}{HP^-_kH^T + R}(HP^-_kH^T + R)K_k^T & \text{代入$(3.4)$}\\ & = P^-_kH^TK_k^T \\ \end{aligned} \]\[\begin{aligned} P_k & = P^-_k - P^-_kH^TK_k^T - K_kHP^-_k + K_k(HP^-_kH^T + R)K_k^T \\ & = P^-_k - P^-_kH^TK_k^T - K_kHP^-_k + P^-_kH^TK_k^T \\ & = P^-_k - K_kHP^-_k \\ & = (I - K_kH)P^-_k \\ \end{aligned} \]\[\Rightarrow P_k = (I - K_kH)P^-_k \tag{3.6} \]其中：
- \(e_{k-1}\) ， \(W_{k-1}^T\)相互独立，\(E(e_{k-1}) = 0\) ， \(E(W_{k-1}^T) = 0\) ，故 \(AE\begin{bmatrix}e_{k-1}W_{k-1}^T\end{bmatrix}=0\)
- \(W_{k-1}\) ， \(e_{k-1}^T\)相互独立， \(E(W_{k-1}) = 0\) ， \(E(e_{k-1}^T) = 0\) ，故 \(E\begin{bmatrix}W_{k-1}e_{k-1}^T\end{bmatrix}A^T = 0\)
\(e_k = X_k - \hat{X}_k\)
\(X_k = AX_{k-1} + BU_{k-1} + W_{k-1}\)
\(W_{k-1}\) 作用在 \(X_k\) 上，即 \(e_k\) 是上一次的，而 \(W_{k-1}\) 是这一次的
故两者没有关系，相互独立
- \(P = E\begin{bmatrix} ee^T \end{bmatrix}\)
- \(Q = E\begin{bmatrix} WW^T \end{bmatrix}\)
那么：
至此，卡尔曼滤波器的五大公式推导完成了
卡尔曼滤波器时间及状态更新方程
时间更新（预测）部分：
- 先验估计值：\[\hat{X}^-_k = A\hat{X}_{k-1} + BU_{k-1} \tag{3.1} \]
- 先验误差协方差矩阵：\[P^-_k = AP_{k-1}A^T + Q \tag{3.5} \]
状态更新（校正）部分：
- 更新卡尔曼增益：\[K_k = \frac{P^-_kH^T}{HP^-_kH^T+R} \tag{3.4} \]
- 后验估计值：\[\hat{X}_k = \hat{X}^-_k + K_k(Z_k - H\hat{X}^-_k) \tag{3.3} \]
- 更新误差协方差矩阵：\[P_k = (I - K_kH)P^-_k \tag{3.6} \]
其中：
需要设置 \(\hat{X}_k\) 的初始值 \(\hat{X}_0\)
以及设置 \(P_k\) 的初始值 \(P_0\)
\(P\)：状态估计误差协方差矩阵，由状态方程（预测方程）确定
\(Q\)：过程噪声协方差矩阵
\(R\)：测量噪声协方差矩阵
\(A\)：状态转移矩阵
\(B\)：状态输入矩阵（控制矩阵）
\(H\)：状态观察矩阵（传输矩阵），表示状态空间到测量空间的转移
\(Z\)：测量矩阵
卡尔曼滤波器的具体使用也就是反复利用上面的五大公式；设好各种矩阵，先时间更新（预测），后状态更新（校正），一直这么反反复复；我们用的值，就是状态估计值\(\hat{X}_k\)
相关矩阵的基本分析
从上面的公式可以看出来，如果 \(P^-_k = 0\) ，那么 \(K_k = 0\) ，可以得到 $\hat{X}_k = \hat{X}^-_k $ ，完全相信估计的值，即不能设置 $P_0 = 0 $ ；
当然给 \(Q\) 赋值可能能解决这个问题，但不能设置太小，会手动引入较大的 \(P^-_k\)，导致估计结果很不平滑；同理 \(R\) 设置太大也会导致这样的结果；
因此，如果初始时设置的不正确，整个估计都是错误的。
\(U、B\) 变量一般来说是被忽略的；
\(Q\) 很难直接的获得，也没有特定的方法，一般设为单位矩阵，或是系统过程比较稳定，也就是没有过程噪声；
\(Z\) 所有状态的的测量值都是由测量仪器测量得到的是已知的；
\(H\) 对于每个状态 \(k\) 都是相同的；
\(R\) 一般来说是相对简单可以求到的，就是测量仪器的噪声协方差；
对于 \(R\) 和 \(Q\) 的取值来说不是特别重要，虽然可能这两个参数估计的不是很准确（不可能完全的符合高斯/正态分布），但是实际情况中卡尔曼滤波器也可以得到整的比较准确态估计，但是需要记住的是：
The better you estimate the noise parameters, the better estimates you get.
你对噪声参数的估计越好，你得到的估计就越好。
那么这样只需要传入基本的初始 \(\hat{X}_0\) 和 \(P_0\) 就可以了，一般来说需要进行指定；
对于 \(\hat{X}_0\) 来说不需要什么技巧，因为随着卡尔曼滤波算法的运行，\(X\) 会逐渐的收敛；
但是对于 \(P_0\) ，一般不要取 \(0\) ，因为这样可能会让卡尔曼滤波器完全相信 \(\hat{X}_k\) 是系统最优的，从而使算法不能收敛。
附录
有关协方差矩阵
若\(W = \begin{bmatrix}w_1 \\\\ w_2\end{bmatrix}\)
\(Q = E\begin{bmatrix}WW^T\end{bmatrix} = E\begin{bmatrix}\begin{bmatrix}w_1 \\\\ w_2\end{bmatrix}\begin{bmatrix}w_1 & w_2\end{bmatrix}\end{bmatrix} = \begin{bmatrix}Ew^2_1 & Ew_1w_2 \\\\ Ew_2w_1 & Ew^2_2 \end{bmatrix}\)
\(\because var(x) = E(x^2) - E^2(x) 且 E(w) = 0\)
\(\therefore var(w) = E(w^2) - E^2(w) = E(w^2)\)
即\(E(w^2) = \sigma_{w^2}\)
\(\because Cov(x,y) = E(xy) - E(x)E(y) 且 E(w_1)=E(w_2)=0\)
也可以这么看：
两个噪声\(w_1,w_2\)是相互独立的，因此\(E(w_1w_2) = E(w_1)E(w_2)\)
\(\therefore Cov(w_1,w_2) = E(w_1w_2) - E(w_1)E(w_2) = E(w_1w_2)\)
即\(E(w_1w_2) = \sigma_{w_1}\sigma_{w_2}\)
\(\therefore Q = \begin{bmatrix}Ew^2_1 & Ew_1w_2 \\\\ Ew_2w_1 & Ew^2_2 \end{bmatrix} = \begin{bmatrix}\sigma_{w_1^2} & \sigma_{w_1}\sigma_{w_2} \\\\ \sigma_{w_2}\sigma_{w_1} & \sigma_{w_2^2} \end{bmatrix}\)
这也与\(II.ii\)中所推得的协方差矩阵的形式一致
因此以\(Q = E\begin{bmatrix}WW^T\end{bmatrix}\)这种表示协方差矩阵是等价的
弹幕中有关\(H\)的争议
对于方程 \(Z_k = HX_k \rightarrow \hat{X}_{{MEA}_k} = H^{-1}Z_k\)
\(H\)是否可逆带有争议
下面是弹幕中的解释
- \(H\)是设计的观测器, 如果不可逆, 有的观测量就⽆意义
- \(H\)是可逆的, 否则系统不具有能观性，即此处⼀定是可观测系统
- \(H\)是单位矩阵, 必须可逆
- \(H\)⼀定可逆, 不然测量⽆意义, 就要重新设计测量⽅案
弹幕关于迹最⼩，⽅差就是最⼩的理解
若\(e = \begin{bmatrix}e_1 \\\\ e_2\end{bmatrix}\)
\(P = E\begin{bmatrix}ee^T\end{bmatrix} = E\begin{bmatrix}\begin{bmatrix}e_1 \\\\ e_2\end{bmatrix}\begin{bmatrix}e_1 & e_2\end{bmatrix}\end{bmatrix} = \begin{bmatrix}Ee^2_1 & Ee_1e_2 \\\\ Ee_2e_1 & Ee^2_2 \end{bmatrix}= \begin{bmatrix}\sigma_{e_1^2} & \sigma_{e_1}\sigma_{e_2} \\\\ \sigma_{e_2}\sigma_{e_1} & \sigma_{e_2^2} \end{bmatrix}\)
\(tr(P) = 对角线相加 = \sigma_{e_1^2} + \sigma_{e_2^2}\)
- 注意，这⾥本身就希望误差的协⽅差最⼩，那么肯定是各误差的协⽅差加起来最⼩，⽽加起来正好等于协⽅差矩阵的迹
- 这⾥可以理解为先特征分解为对⻆，特征值的和就是迹
迹⾥⾯包含⽅差，迹最⼩，⾃然⽅差最⼩ - 为什么取迹最⼩，我的理解是，这样只是算了所有变量的⽅差和，不考虑变量间的关系。
- 卡尔曼滤波的前提条件是初始状态以及每⼀时刻的噪声都认为是互相独⽴的，根据误差合成理
论，协⽅差为零，所以这⾥就是迹 - 因为要找⽅差最⼩ ⽽刚刚好P矩阵的迹就是⽅差 所以就是最⼩
迹是⽅差的平⽅和，迹最⼩⽅差也最⼩ - 难道这就是最⼩⼆乘法估计？？
- 加起来的和最⼩的情况是两个都是最⼩的，这⾥e1e2没有关系，都能得到最⼩值，不存在此消彼
⻓的情况 - ⾮对⻆线的数值的意义是各元素间的关系，所以其实不⽤考虑关系的⽅差
有关\(\frac{dtr(AB)}{dA} = B^T\)
设\(A = \begin{bmatrix}a_{11} & a_{12} \\\\ a_{21} & a_{22} \end{bmatrix} \quad B = \begin{bmatrix}b_{11} & b_{12} \\\\ b_{21} & b_{22} \end{bmatrix}\)
\( \begin{aligned} AB & = \begin{bmatrix}a_{11} & a_{12} \\\\ a_{21} & a_{22} \end{bmatrix} \begin{bmatrix}b_{11} & b_{12} \\\\ b_{21} & b_{22} \end{bmatrix} \\ & = \begin{bmatrix}a_{11}b_{11} + a_{12}b_{21} & \cdots \\\\ \cdots & a_{21}b_{12}+a_{22}b_{22} \end{bmatrix} \end{aligned} \)
那么 \(tr(AB) = a_{11}b_{11} + a_{12}b_{21} + a_{21}b_{12}+a_{22}b_{22}\)
\( \begin{aligned} \frac{dtr(AB)}{dA} & = \begin{bmatrix}\frac{\partial tr(AB)}{\partial a_{11}} & \frac{\partial tr(AB)}{\partial a_{12}} \\\\ \frac{\partial tr(AB)}{\partial a_{21}} & \frac{\partial tr(AB)}{\partial a_{22}} \end{bmatrix} \\ & = \begin{bmatrix}b_{11} & b_{21} \\\\ b_{12} & b_{22} \end{bmatrix} \\ & = B^T \end{aligned} \)
标签：begin,end,KF,卡尔曼滤波,kH,bmatrix,-_,hat From： https://www.cnblogs.com/champrin/p/17013782.html