GPS原理与接收机设计 CH6 卡尔曼滤波及其应用

CH6 卡尔曼滤波及其应用

第六章主要是讲卡尔曼滤波的原理 公式推导 和应用
感觉还是有难度的 所以在前期有陆陆续续地拆解 然后也上传了blog 需要的uu可以看看哦~
α-β滤波
通俗讲讲 卡尔曼滤波
卡尔曼滤波
注：看了上面的几个链接可以跳过6.2 到 6.3.3 的内容噢~

本文的思维导图出炉~~
⭐️需要的uu可以免费下载领取噢~
https://download.csdn.net/download/qq_53131867/88541387

⭐️章节概要
滤波的意义
由于前面的最小二乘法只将单独某个时刻的的位置定位，不具有连续性，所以的出来的定位比较粗糙，所以就提出了滤波 ~~ 希望能将行踪变得平缓，α-β滤波和卡尔曼滤波就是把用户相邻时刻的位置状态联系起来，克服最小二乘法在不同时刻定位值不相关联的缺点，使得滤波后的定位结果变得平滑准确 ~
⛵️α-β滤波
α-β滤波是卡尔曼滤波的一种稳态形式，它默认上一时刻和下一时刻的速度是一样的，用于平滑用户的运动速度。然后介绍了它的缺点，只能在用户的一个坐标份力进行滤波，这使得用户位置或者速度什么的分量之间失去了应有的相互联系。
卡尔曼滤波
⭐️这节讲的很多，也很重要！
可以说，谢钢老师这本书讲的很详细了，从算法的整个模型出发，讲了公式的推导，滤波数值的计算，以及它的一些缺点，扩展性卡尔曼滤波等。
系统模型的建立
主要是对于连续时间系统 和离散时间系统进行建模
（这里用到了信号与系统的拉普拉斯变换）
GPS定位的卡尔曼滤波算法
也就是基于GPS 来给你分析一下卡尔曼滤波怎么用
其他滤波技术
讲了卡尔曼滤波的优缺点 以及其他滤波模型 比如粒子滤波 多态自适应卡尔曼滤波

​ 上一章介绍了GPS定位原理和最小二乘法定位算法。尽管最小二乘法能在含有误差与噪声的各个测量值之间寻求-一个最优点，使得所有测量值的残余平方和最小，但是由于不同时刻的不同测量误差与噪声在最小二乘法计算后转化为相应时刻的不同定位误差与噪声,因而最小二乘法的定位结果通常显得既粗糙又杂乱。 滤波是一种降低、分离信号中所含噪声量的技术，而这一章将详细讲解卡尔曼滤波技术及其作为另一-种GPS定位算法的应用。

6.1 滤波的意义

​ 导航采用滤波技术的根本原因：物体的惯性

​ 行踪是连续性的 会有一定的缓冲——物体运动的连续性和运动变化的缓慢性~

​ 所以定位导航系统面对所有物体运动都要尊重这一事实

​ 滤波：保留所需要的信号成分

​ α-β滤波和卡尔曼滤波——把用户相邻时刻的位置状态联系起来，克服最小二乘法在不同时刻定位值不相关联的缺点，使得滤波后的定位结果变得平滑准确~

6.2 α-β滤波

是卡尔曼滤波的一种稳态的形式

α-β滤波器

假设一列标量
$${\tilde{x}}_1,{\tilde{x}}_2,{\tilde{x}}_3,\cdots$$
是测量到的随时间变化的用户位置在某坐标系中的其中一个坐标分量值，（比如是WGS-84地心地固直角坐标系中的x分量或者是大地坐标系中的纬度值等）而在上一章中计算得到的GPS接收机最小二乘法（或加权最小二乘法）定位解在此也可以视为一种位置测量值。因为这一列测量值或者由最小二乘法得到的定位解一般很粗糙，所以我们希望它们经过滤波后看起来会变得平滑些，并且滤波结果通常也会更加接近于物体的真实位置坐标值。

假设$\hat{x}$_k-1（位置滤波值）和${\hat{\dot{x}}}_{k-1}$（速度滤波值）是α-β滤波器在得到第k -1个位置测量值$\tilde{x}$_k-1后的滤波结果，其中$\hat{x}$_k-1是滤波器对用户真实位置x_k-1的估计值，而${\hat{\dot{x}}}_{k-1}$是滤波器对用户真实速度$\dot{x}$_k-1的估计值，那么α-β滤波器先用速度值${\hat{\dot{x}}}_{k-1}$来预测用户在第k个测量时刻的位置，即
$${\hat{x}}_k^{-}={\hat{x}}_{k-1}+{\hat{\dot{x}}}_{k-1}{T}_s(6.1)$$
式中，T_s代表相邻两个测量时刻之间的时间间隔。我们用上标“ $\hat{}$ ”代表估计值 ，以区别于上标 “~”所代表的实际测量值， 而中的右上标“-”代表滤波器在得到、利用测量值之前的先验估计值。

上面可能看的花了都 /(ㄒoㄒ)/~~ 简化一下

重新整理一下：

s v 是对前一个位置测量值 c （d是它的真实位置）的滤波结果，其中s是滤波器对于用户真实位置d的估计值 v是真实速度 e 的估计值 那么α-β滤波器就是先用速度值v来预测一下用户在下一个时刻的位置

然后这个式子可以这么理解

下一时刻的位置估计值=上一时刻的位置+上一时刻的速度*两时刻间隔的时间

（相当于A和B两个地点 A————B）

B的大致位置坐标=A的位置坐标+速度*A到B之间的时间（他假定了速度是匀速的 也就是前后两个时刻的速度不发生变化）

关于先验估计值

先验概率（prior probability）： 指根据以往经验和分析。在实验或采样前就可以得到的概率。 后验概率（posterior probability）： 指某件事已经发生，想要计算这件事发生的原因是由某个因素引起的概率。

可以看出，先验概率就是事先可估计的概率分布，而后验概率类似贝叶斯公式“由果溯因”的思想。

具体可以看看这篇文章：<基础系列>1：先验概率 & 后验概率 - 笨笨的文章 - 知乎
https://zhuanlan.zhihu.com/p/38567891

式(6.1)表明，α-β滤波器假定了用户在每一测量间隔时段内的运动速度保持不变，即速度先验估计值效${\hat{x}}_k^{-}$为
$${\hat{x}}_k^{-}={\hat{x}}_{k-1}(6.2)$$
在得到第k历元的位置测量值后，α-β滤波器用下面两个公式更新对当前时刻用户位置和速度的估计值:
$$\begin{array}{rlr}{\hat{x}}_k& ={\hat{x}}_k^{-}+\alpha ({\tilde{x}}_k-{\hat{x}}_k^{-})& (6.3)\\ {\hat{\dot{x}}}_k& ={\hat{\dot{x}}}_k^{-}+\frac{\beta }{T_s}({\tilde{x}}_k-{\hat{x}}_k^{-})& (6.4)\end{array}$$

也就是（用户最新测量位置值=用户位置先验估计的值+α（用户最新测量位置值-用户位置先验估计的值））

式中,α和β为两个值固定的滤波系数。由式(6.3)可见，a-β滤波器试图在位置先验估计值${\hat{x}}_k^{-}$与最新得到的测量值${\tilde{x}}_k$之间做出平衡，而式(6.4)对速度也做了类似的考量。系数α与β的值越大，则滤波器对当前测量值的权重就越高。α-β滤波器稳定的充分必要条件如下:
$$0<\alpha ,0<\beta <4-2\alpha (6.5)$$
通常，滤波系数α和β均取值为小于1的正数。在适当的前提假设下，α 与β的最优化值之间存在着一定的关系。

式(6.1)~式(6.4)可用矩阵形式简洁地表达为：
$$\begin{array}{rlr}{\hat{\mathbf{x}}}_k^{-}& =\left[\begin{array}{cc}1& T_s\\ 0& 1\end{array}\right]{\hat{\mathbf{x}}}_{k-1}& (6.6)\\ {\hat{\mathbf{x}}}_k& ={\hat{\mathbf{x}}}_k^{-}+\left[\begin{array}{c}\alpha \\ \beta /T_s\end{array}\right]({\tilde{\mathbf{x}}}_k-[10]{\hat{\mathbf{x}}}_k^{-})& (6.7)\end{array}$$
其中，向量${\hat{x}}_k^{-}=[{\hat{x}}_k^{-}{\hat{\dot{x}}}_k^{-}{]}^{\mathrm{T}}$而${\hat{x}}_k=[{\hat{x}}_k^{-}{\hat{\dot{x}}}_k{]}^{\mathrm{T}}$

上面这个式子把前面的两个当前时刻用户位置和速度的估计值式子 分别合并了（用0和1 代表加号和减号 ）

我们将在6.3.3节对以上的a-β滤波计算公式与卡尔曼滤波的稳态计算公式进行对比。

上述α-β位置滤波器根据用户在每一小段时间内以匀速运动的假设来平滑位置测量值，同样α-β滤波器也可以用来平滑用户的运动速度。

如果速度测量值的一个坐标分量作为α-β滤波器的输入，那么α-β速度滤波器实际上是以一个恒加速度的运动模型来对此速度分量测量值进行滤波的。

a-β-γ滤波器

与α-β滤波器属于同一类型的还有α滤波器和a-β-γ滤波器。

a-β-γ位置滤波器假定用户运动的加速度在一小段时间内不变，而α位置滤波器则不再估算物体的运行速度或加速度。我们在5.6节介绍GPS定速算法时曾指出、接收机给出的用户速度值通常比位置值更精确。因此，为了利用接收机速度测量值的这种优越性,我们可将前面α-β滤波器中的速度诂计值${\hat{\dot{x}}}_{k-1}$直接用相应时刻的接收机速度测量值替代,而此时的a-β滤波器实际上变成了α位置滤波器。因为α滤波器简单地借用了速度测量值来平滑位置测量值，所以它经常又被称为平滑器。事实上，第4章中以式(4.84)所代表的载波相位平滑伪距的方法正是应用了这种平滑器。α滤波的另一种形式是当速度值在理论上等于零的情况，此时式(6.3)变为
$${\hat{x}}_k={\hat{x}}_{k-1}+\alpha \left({\tilde{x}}_k-{\hat{x}}_{k-1}\right)=(1-\alpha ){\hat{x}}_{k-1}+\alpha {\tilde{x}}_k,(6.8)$$
即滤波值${\hat{x}}_k$是对一系列静态测量值${\hat{x}}_k$进行平滑的结果。

α-β滤波技术的缺点之一是它的一维滤波。由于一个α-β滤波器只对用户位置(或速度)的一个坐标分量单独进行滤波,因而一个三维运动的滤波问题就需要三个相互独立运行的α-β滤波器,这使得用户位置(或速度)的三个分量之间失去了应有的相互联系。

无论是α滤波器、α-β滤波器，还是a-β-γ滤波器，它们均用一个恒定的滤波系数对用户运动状态的先验估计值与实际测量值进行加权平均，而没有考虑到在不同时刻的测量值可能有着不同大小的测量误差，这是α-β滤波器的又一缺点。对于这两个问题，卡尔曼滤波技术均有效地给予了解决。

6.3 卡尔曼滤波

6.3.1 滤波模型

卡尔曼滤波所要解决的问题是如何对一个离散时间线性系统的状态进行最优估算,而这一小节将先从这类线性系统的卡尔曼滤波模型谈起。

对于计算量 有数学建模等的误差

对于测量量 有测量工具的误差 人为误差等等

也就是说卡尔曼滤波是要寻找一个值平衡计算出来的计算量与测量出来的测量量

⭐️也就是一个数据融合的过程 那么这个值如何寻找，就是接下去的内容

考虑一个有多个输入量(又称控制量)和多个输出量(又称观测量)的离散时间线性动态系统，其控制过程一般可用以下的线性差分方程式来表示:
$$x(t_k)=A(t_k,t_{k-1})x(t_{k-1})+B(t_{k-1})u(t_{k-1})+w(t_{k-1})(6.9)$$
上式称为状态方程

式中t_k 代表第k个测量时刻或第k测量历元所对应的时间，u 代表系统的输入向量，x 代表系统的状态向量,w 代表过程噪声向量，$A(t_k,t_{k-1})$代表从t_k-1到t_k时刻的状态转移矩阵，$B(t_{k-1})$代表在t_k-1时刻系统输入量与系统状态之间的关系矩阵。

也就是

系统在k历元的状态量=从k-1到k时的状态转移矩阵系统在k-1历元的状态量 + k-1时刻系统输入量和系统状态之间的关系矩阵* * k-1历元时的系统的输入量 + 噪声量

系统的输入量u是个可选项，即有些系统可以没有输入量，比如用户GPS 接收机这个定位系统通常视为没有任何输入。

状态转移矩阵$A(t_k,t_{k-1})$与输入关系矩阵$B(t_{k-1})$在不同时刻可以是不同的，但为了简化公式表达，以后我们认为它们的值固定不变。这样，式(6.9)可简写成

☘️6.9是长这样滴
$$x(t_k)=A(t_k,t_{k-1})x(t_{k-1})+B(t_{k-1})u(t_{k-1})+w(t_{k-1})(6.9)$$

$$x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}(6.10)$$
式中，x 代表x(t_k)，而其余各个参量的含义与此相类似。式(6.10)称为卡尔曼滤波的状态方程，它描述了系统状态如何随时间变化，是系统的动态模型，而图6.1的左半部分等价地描述了这一系统的控制过程。

由多个状态变量组成的状态向量x_k,全面描述了系统在当前时刻的运行状况，它们的值通常是未知的，但一般却是我们所想要了解、掌握的。各个状态变量必须具有可观性，即它们的值能直接或间接地反映在对系统的观测量之中 ,而我们应用卡尔曼滤波器的目的正是为了达到从系统观测量y_k来估算系统状态x_k。

卡尔曼滤波假定系统的状态向量x_k与观测向量y_k存在以下线性关系:
$$y_k=C{x}_k+{\nu }_k(6.11)$$

上式为测量方程

系统观测量=观测量与系统状态之间的关系矩阵 * 系统状态量 +噪声量

其中，C 代表观测量与系统状态之间的关系矩阵，v 代表测量噪声向量。式(6.11)称为卡尔曼滤波的测量方程 ，它描述了当前的系统测量值与系统状态之间的关系，而图6.1的右半部分也等价地描述了这一系统的测量过程。尽管卡尔曼滤波允许测量关系矩阵的值随时间而变化，但是为了简化起见，我们在上式中将它视为一个常系数阵C。

卡尔曼滤波用上述的一个状态方程式(6.10)和一个测量方程式(6.11)来完整描述一个线性动态系统，它们是该线性动态系统的数学模型。 对于一个有L个输入变量、M个观测变量以及N个状态变量的系统，卡尔曼滤波器涉及以下一些系统参变量:

(1) $x_k$ :一个由N个状态变量$(x_{1,k},x_{2,k},\cdots ,x_{N,k})$所组成的N×1的状态向量。

(2) $u_k$ :一个由L个输入变量$(u_{1,k},u_{2,k},\cdots ,u_{L,k})$所组成的L×1的输入向量。

(3) $y_k$:一个由M个观测变量$(y_{1,k},y_{2,k},\cdots ,y_{M,k})$所组成的M×1的观测向量。

(4)$w_k$ :一个由N个过程噪声$(w_{1,k},w_{2,k},\cdots ,w_{N,k})$所组成的Nx1的过程噪声向量。

(5) $v_k$ :一个由M个测量噪声$(v_{1,k},v_{2,k},\cdots ,v_{M,k})$所组成的M×1的测量噪声向量。

(6) $A$ :一个NxN的状态转移矩阵。

(7) $B$:一个N×L的输入关系矩阵。

(8) $C$:一个M×N的测量关系矩阵。

这个图再放一下，唤醒一下记忆~

**向量$w_k$**中的各个过程噪声变量代表系统输入量$u_k$所包含的以及由系统内部所产生的随机噪声误差。卡尔曼滤波假定向量${\mathbf{w}}_{\mathbf{k}}$中的每个过程噪声变量为一个均值为零的正态白噪声，即
$$\begin{array}{rlr}E({\mathbf{w}}_k)& =0& (6.12)\\ \mathrm{Cov}({\mathbf{w}}_k)& =E({\mathbf{w}}_k{\mathbf{w}}_k^{\mathrm{T}})=\mathbf{Q}& (6.13)\end{array}$$

这里关于协方差的知识做一个补充

具体可以看这个blog：

https://blog.csdn.net/shijin1996/article/details/105581052/

$Cov(X,Y)=E[(X-{\mu }_x)(Y-{\mu }_y)]$

该公式可以有如下理解：如果有X,Y两个变量，每个时刻的“X值与其均值之差”乘以“Y值与其均值之差”得到一个乘积，再对这每时刻的乘积求和并求出均值.

1.协方差可以反应两个变量的协同关系， 变化趋势是否一致。同向还是方向变化。
2.X变大，同时Y也变大，说明两个变量是同向变化的，这时协方差就是正的。
3.X变大，同时Y变小，说明两个变量是反向变化的，这时协方差就是负的。
4.从数值来看，协方差的数值越大，两个变量同向程度也就越大。反之亦然。

在协方差中，当只有一个变量（如x）时，cov(x)表示该变量与自身的协方差。协方差衡量两个随机变量之间的线性关系强度和方向。

这里是和自身的协方差

与自身的协方差通常称为变量的方差。方差衡量一个随机变量在其期望值附近的变动范围，是随机变量离其期望值的平均偏离程度的平方。

具体而言，与自身的协方差表示一个变量自己的变化程度。如果方差较大，则表明随机变量的取值相对较分散或离散；而方差较小，则表明随机变量的取值相对集中或稳定。

也就是说$w_k$，的概率分布呈N(0,Q)。 过程噪声向量$w_k$的协方差矩阵Q为一个N×N的对称矩阵，但它并不一定是个对角阵。
向量$v_k$中的各个测量噪声变量代表系统观测量$y_k$所包含的随机测量误差与噪声。卡尔曼滤波假定每个测量噪声变量也是一个均值为零的正态白噪声，即
$$\begin{array}{rlr}E({\nu }_k)& =0& (6.14)\\ \mathrm{Cov}({\nu }_k)& =E({\nu }_k{\nu }_k^{\mathrm{T}})=R& (6.15)\end{array}$$
也就是说，$v_k\sim N(0,R)$。 同样，测量噪声向量$v_k$的协方差矩阵R为一个M×M 的对称矩阵，但它并不一定是个对角阵。

尽管噪声向量$w_k$和$v_k$的值是未知的,但它们的协方差矩阵Q和R对卡尔曼滤波器来说是已知的。尽管协方差矩阵Q和R的值一般会随着时间的不同而变化，但是为了简化公式表达，我们这里认为它们均是一个常系数矩阵。此外，卡尔曼滤波假定过程噪声向量$w_k$中的各分量与测量噪声向量$v_k$中的各分量互不相关，即
$$E(w_i{v}_j^{\mathrm{T}})=\theta$$
其中，整数 $i$ 和 $j$ 均为任一有效历元。

6.3.2 滤波算法

整个理解

对系统的状态进行最有估计 使系统状态的估计值具有最小 均方误差

1️⃣从状态方程（也就是给定的数学模型）我们求解出一个值 ${\hat{x}}_k^{-}$ 此时系统状态真实值和它的差就是我们的先验估计误差

（比如我们跑步 已知速度的情况下，有两种方式可以测量加速度，️粗略地根据a=dv/dt可以大致测量出我们的加速度 (但是这个加速度肯定是不太准确的 我们没有考虑其他因素的影响 数学建模得到的关系式可能还不够精准) ️ 借用手环上的加速度计来进行辅助 直接从仪器上得到我们 的加速度 ）

这个时候我们的先验估计值也就是a方案得出来可以求解的

2️⃣这个时候我们借助测量量来使得我们得到的状态值尽可能准确 那么怎么样将我们的先验状态估计值和测量值建立关系 从而得到一个相对准确的量？？ 于是卡尔曼建立了一个方程 构建这两者之间的关系 后验估计值=先验估计值+K(测量量-C*先验估计值) 而我们的目标就是让这个后验估计值足够的准确。

而我们的测量值 就是手环直接给出的

虽然是手环测出来的，但是手环上也可能有仪器方面的误差

那我们应该相信哪一个？？

于是就有了这个方程

后验估计值=先验估计值+K(测量量-C*先验估计值)
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k{r}_k={\hat{x}}_k^{-}+K_k(y_k-C{\hat{x}}_k^{-})$$
我们的后验估计值，就是综合了手环上的测量量和数学模型的出来的计算量得出来的一个比较准确的量

这也是数据融合的过程~

3️⃣ 问题来了，足够准确用什么来评定？这里采用的是最小均方误差，让我们的误差足够小，不就可以反向论证 我们的后验估计值能够足够准确了吗 ？ 于是目标变成 找到一个K使得我们的整个过程具有最小的均方误差！！

4️⃣ 于是啊，我们就要求解K和e之间的关系

而反映e的矩阵就是我们的P 它各对角线元素分别对应于各状态变量估计值的均方误差 所以我们就要求解P和K之间的关系 ➡️找到一个K使得我们的整个过程具有最小的均方误差！！ 于是找到P和K的关系式之后再进行求导等等等等 一系列操作 ，就得到了K的最优值！！ 以及此时的P的最小值 ~~任务完成！

正题

卡尔曼滤波用一套数学递推公式对系统状态进行最优估计,使系统状态的估计值有最小均方误差（MMSE )。 假设${\hat{x}}_{k-1}$ 代表第k-1个历元时卡尔曼滤波器对系统状态$x_{k-1}$的最优估计值，那么在同时给出输入量$u_{k-1}$和观测量$y_k$的条件下，我们在这一小节来推导第k个历元时卡尔曼滤波器对系统状态$x_k$的最优估计值${\hat{x}}_k$。与6.2节中的表达方式相同，上标“^”依然代表估计值。

从${\hat{x}}_{k-1}$和 ${\hat{u}}_{k-1}$出发，卡尔曼滤波利用状态方程式(6.10)来预测第k历元时的状态$x_k$ ，即
$${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}(6.17)$$

6.10的式子 卡尔曼滤波的状态方程
$$x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}(6.10)$$

因为估计值${\hat{x}}_k^{-}$还没有得到测量值$y_k$的验证，所以${\hat{\mathbf{x}}}_{\mathbf{k}}^{-}$通常被称为先验估计值，并用右上标“-”代表先验。先验估计误差${\hat{\mathbf{e}}}_{\mathbf{k}}^{-}$定义为系统状态的真实值$x_k$与其先验估计值${\hat{x}}_k^{-}$之间的差异，即
$$e_k^{-}=x_k-{\hat{x}}_k^{-}(6.18)$$
先验估计误差${\hat{e}}_k^{-}$的协方差阵${\hat{P}}_k^{-}$称为状态均方误差阵（或称误差协方差阵)，它可直接根据协方差的定义而表达成
$$P_k^{-}=E(e_k^{-}{e}_k^{-\mathrm{T}})(6.19)$$
有了对当前状态$x_k$的先验估计值${\hat{x}}_k^{-}$，我们就可根据测量方程式(6.11)而认为第k历元的观测量值应该等于$C{\hat{x}}_k^{-}$ 再加上未知的测量噪声$v_k$。观测量的实际值$y_k$与其预测值$C{\hat{x}}_k^{-}$之间的差异，即
$$r_k=y_k-C{\hat{x}}_k^{-}(6.20)$$

回顾一下6.11
$$y_k=C{x}_k+{\nu }_k(6.11)$$

$r_k$称为观测量的残余。接着，卡尔曼滤波将先验估计值${\hat{x}}_k^{-}$与观测量残余$r_k$的线性组合作为对状态$x_k$的最优估计值${\hat{x}}_k$， 即
$${\hat{x}}_k={\hat{x}}_k^{-}+K_k{r}_k={\hat{x}}_k^{-}+K_k(y_k-C{\hat{x}}_k^{-})(6.21)$$
其中，系数矩阵$K_k$称为卡尔曼滤波增益。因为实际观测量$y_k$已经核对、校正了估计值${\hat{x}}_k$，所以${\hat{x}}_k$通常称为后验估计值。

类似于式(6.18)和式(6.19)，我们可定义后验估计值${\hat{x}}_k$的误差$e_k$为
$$e_k=x_k-{\hat{x}}_k(6.22)$$
定义后验估计误差$e_k$的均方误差阵$P_k$为
$$P_k=E(e_k{e}_k^{\mathrm{T}})(6.23)$$
上述后验估计均方误差阵P_k中的各个对角线元素分别对应于各个状态变量估计值的均方误差。下面要做的是推导出增益矩阵K_k的最优值，从而使得均方误差阵Р_k的对角线元素之和最小，那么由此得到的卡尔曼滤波状态估计值${\hat{x}}_k$就有最小均方误差，相应地卡尔曼滤波器在此意义上也就是一种最优的滤波器。

为了推导$K_k$ ，我们得先对$e_k$与$P_k$进行一些整理。将式(6.11)、式(6.18)和式(6.21)代入式(6.22)，经整理后
$$\begin{gathered} y_k=C{x}_k+{\nu }_k(6.11) \\ {e}_k^{-}=x_k-{\hat{x}}_k^{-}(6.18) \\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k{r}_k={\hat{x}}_k^{-}+K_k(y_k-C{\hat{x}}_k^{-})(6.21) \\ {e}_k=x_k-{\hat{x}}_k(6.22) \end{gathered}$$
得
$$e_k=(I-K_{k}C)e_k^{-}-K_k{\nu }_k(6.24)$$

注 其中$I$是指的是单位矩阵 不同课本也用E表示单位矩阵

具体推导的过程

再将（6.15） （6.19） （6.24）代入（6.23）
$$\begin{gathered} \begin{array}{rlr}E({\nu }_k)& =0& (6.14)\\ \mathrm{Cov}({\nu }_k)& =E({\nu }_k{\nu }_k^{\mathrm{T}})=R& (6.15)\end{array} \\ {P}_k^{-}=E(e_k^{-}{e}_k^{-\mathrm{T}})(6.19) \\ {P}_k=E(e_k{e}_k^{\mathrm{T}})(6.23) \end{gathered}$$
得
$${\mathbf{P}}_{\mathbf{k}}={\mathbf{P}}_k^{-}-{\mathbf{K}}_k\mathbf{C}{\mathbf{P}}_k^{-}-{\left({\mathbf{K}}_k\mathbf{C}{\mathbf{P}}_k^{-}\right)}^{\mathrm{T}}+{\mathbf{K}}_k\left(\mathbf{C}{\mathbf{P}}_k^{-}{\mathbf{C}}^{\mathrm{T}}+\mathbf{R}\right){\mathbf{K}}_k^{\mathrm{T}}(6.25)$$

关于计算的过程 我也放了一个参考的

$${\mathbf{P}}_{\mathbf{k}}={\mathbf{P}}_k^{-}-{\mathbf{K}}_k\mathbf{C}{\mathbf{P}}_k^{-}-{\left({\mathbf{K}}_k\mathbf{C}{\mathbf{P}}_k^{-}\right)}^{\mathrm{T}}+{\mathbf{K}}_k\left(\mathbf{C}{\mathbf{P}}_k^{-}{\mathbf{C}}^{\mathrm{T}}+\mathbf{R}\right){\mathbf{K}}_k^{\mathrm{T}}(6.25)$$

在对式(6.25)的推导过程中，我们需要利用到先验估计误差$e_k^{-}$与当前历元的测量噪声$v_k$之间互不相关的假设。

为了对上式中P_k的对角线元素之和进行求导，我们得借助以下两个求导公式
$$\begin{array}{c}\frac{\mathrm{d}\left(\mathrm{tr}(\mathbf{FG})\right)}{\mathrm{d}\mathbf{F}}={\mathbf{G}}^{\mathrm{T}}\text{(6.26)}\\ \frac{\mathrm{d}\left(\mathrm{tr}({\mathbf{FHF}}^{\mathrm{T}})\right)}{\mathrm{dF}}=2\mathbf{FH}(6.27)\end{array}$$
其中，F,G和H可为任意矩阵，但是式(6.26)中的乘积FG必须为方阵，而式(6.27)中的H必须为对称阵。运算符“tr”代表矩阵的求迹算子，即计算矩阵的对角线元素之和。

具体推导

式(6.25)表明，P_k或者说tr(P_k)是一个关于K_k的二次型函数。因为$((\mathbf{C}{\mathbf{P}}_k^{-}{\mathbf{C}}^{\mathrm{T}}+\mathbf{R})$正定，所以关于K_k的二次型函数 tr(P_k)存在最小值。将tr(P_k)对K_k求导，并使其导数值等于零，我们就可得到使tr(P_k)值最小的K_k的解，即
$$\frac{\mathrm{d}\left(\mathrm{tr}({\mathbf{P}}_k)\right)}{\mathrm{d}{\mathbf{K}}_k}=-2{\left(\mathbf{C}{\mathbf{P}}_k^{-}\right)}^{\mathrm{T}}+2{\mathbf{K}}_k\left(\mathbf{C}{\mathbf{P}}_k^{-}{\mathbf{C}}^{\mathrm{T}}+\mathbf{R}\right)(6.28)$$

$$K_k=P_k^{-}{C}^{\mathrm{T}}{\left(C{P}_k^{-}{C}^{\mathrm{T}}+R\right)}^{-1}(6.29)$$

再将式(6.29)中的K_k代回到式(6.25)，得
$$P_k=(I-K_{k}C)P_k^{-}(6.30)$$

$${\hat{x}}_k={\hat{x}}_k^{-}+K_k{r}_k={\hat{x}}_k^{-}+K_k(y_k-C{\hat{x}}_k^{-})(6.21)$$

回到式（6.21）结合式（6.30）和（6.29），我们可以得到：

当测量误差R(测量噪声）很小的时候，得到的就是测量结果。当测量误差R很大的时候，就是我们的估计（用公式/数学模型等）计算的结果。

总结

在此，我们将卡尔曼滤波的递推算法总结一下。如图6.2所示，卡尔曼滤波算法可分为预测和校正两个过程。

(1)预测 :预测过程又叫时间更新过程，它在上一(即第k-1)个历元状态估计值的基础上，利用系统的状态方程来预测当前这一（即第k )个历元的状态值。这一过程涉及以下两个公式:
$$\begin{array}{rlr}{\hat{\mathbf{x}}}_k^{-}& =A{\hat{\mathbf{x}}}_{k-1}+B{\mathbf{u}}_{k-1}& (6.31)\\ {\mathbf{P}}_k^{-}& =A{\mathbf{P}}_{k-1}{A}^{\mathrm{T}}+\mathbf{Q}& (6.32)\end{array}$$

对于6.32 在这里推导一下 可能一开始会很懵
$$\begin{gathered} x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}(6.10) \\ \begin{array}{rlr}E({\mathbf{w}}_k)& =0& (6.12)\\ Cov({\mathbf{w}}_k)& =E({\mathbf{w}}_k{\mathbf{w}}_k^{\mathrm{T}})=\mathbf{Q}& (6.13)\end{array} \\ {P}_k^{-}=E(e_k^{-}{e}_k^{-\mathrm{T}})(6.19) \\ {e}_k^{-}=x_k-{\hat{x}}_k^{-}(6.18) \\ {\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}(6.17) \end{gathered}$$

其中，式(6.31)只是重复了式(6.17)，它利用状态方程来预测当前的系统状态。在卡尔曼滤波中，每一个状态估计值必须跟随一个用来衡量k此状态估计值可靠性的均方误差阵。 在预测状态时，由于过程噪声w_k的值未知，因而状态先验估计值的均方误差阵计算公式(6.32)添加了过程噪声的协方差Q以降低状态先验估计值的可靠性和保持均方误差阵的正确性。在完成这一预测过程后，系统的状态估计均方误差通常会变大。

(2)校正: 校正过程又叫测量更新过程，它是利用实际测量值来校正经上一步预测得到的状态先验估计值。这一过程涉及以下三个公式:
$$\begin{array}{c}{K}_k=P_k^{-}{C}^{\mathrm{T}}{\left(C{P}_k^{-}{C}^{\mathrm{T}}+R\right)}^{-\mathrm{l}}\text{(6.33)}\\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k\left({\mathbf{y}}_k-\mathbf{C}{\hat{\mathbf{x}}}_k^{-}\right)\text{(6.34)}\\ P_k=(I-K_{k}C)P_k^{-}\text{(6.35)}\end{array}$$
其中，式(6.33)就是我们前面推导出的卡尔曼增益计算公式(6.29)，而式(6.34)和式(6.35)对系统的状态估计值及其均方误差进行更新。

在这一过程中，由于状态估计值得到了实际测量值的验证，因而它的均方误差值变小，即可靠性增加。 卡尔曼滤波器综合、平衡了它的先验估计和实际测量这两方面的信息，使测量更新以后的状态估计值具有最小的均方误差。

⭐️ ​关于初值

如图6.2所示，卡尔曼滤波器需要外界提供系统状态及其均方误差的初始估计值，并且要求状态初始估计值为当时状态真实值的均值，即
$${\hat{x}}_0=E(x_0)(6.36)$$
而相应的状态均方误差的初始估计值P₀满足
$$P_0=E((x_0-{\hat{x}}_0)(x_0-{\hat{x}}_0{)}^{\mathrm{T}})(6.37)$$
卡尔曼滤波不但给出系统状态的估计值${\hat{x}}_k$，而且还同时给出这个估计值的均方误差P_k。可以证明，系统状态真实值x_k的均值与方差分别等于后验估计值${\hat{x}}_k$与P_k，即
$$\begin{array}{rlr}E({\mathbf{x}}_k)& ={\hat{\mathbf{x}}}_k& (6.38)\\ E({\mathbf{e}}_k{\mathbf{e}}_k^{\mathrm{T}})& ={\mathbf{P}}_k& (6.39)\end{array}$$

$${\hat{x}}_k={\hat{x}}_{k-1}+\alpha \left({\tilde{x}}_k-{\hat{x}}_{k-1}\right)=(1-\alpha ){\hat{x}}_{k-1}+\alpha {\tilde{x}}_k,(6.8)$$
其中，式(6.8)表明了卡尔曼滤波器可提供系统状态的无偏估计。 代表状态估计值可靠性的均方误差P_k，其重要性并不亚于状态估计值${\hat{x}}_k$。如果状态估计的均方误差Р_k过大，比如超过了一个门限值，那么我们应该对此时的滤波结果${\hat{x}}_k$保持警惕，甚至放弃利用该滤波结果。 卡尔曼滤波的这种同时提供${\hat{x}}_k$和P_k的特点在各种实际应用中非常受欢迎，而这一特点是 α-β 滤波器所没有的。

卡尔曼滤波器是一种常用于估计系统状态的滤波器。 它的主要优点在于能够提供系统状态的无偏估计，即后验估计值的期望值等于真实值。

卡尔曼滤波器的无偏性是由其基本假设所决定的，即系统状态是通过一个线性动态系统生成的，同时噪声也服从高斯分布。在这种情况下，后验估计值的期望值等于真实值。

系统状态真实值的均值与方差分别等于后验估计值与 P 的原因是卡尔曼滤波器采用递归的方式， 利用前一个时刻的状态估计值与观测值来计算下一个时刻的状态估计值。因此，每次更新状态估计值后，系统状态真实值的均值与方差都会按先验估计值和观测值的方差来进行更新，因此后验估计值与P会逐步接近真实值的均值与方差。

无偏估计

估计是用样本统计量（可以理解为随机抽样）来估计总体参数时的一种无偏推断。无偏估计的要求就是：估计出来的参数的数学期望等于被估计参数的真实值。无偏估计的意义：在多次重复下，估计量的平均值 ≈ 被估计参数真值

具体可以参考这篇blog： https://blog.csdn.net/u012370185/article/details/94590902

$$\begin{gathered} K_k=P_k^{-}{C}^{\mathrm{T}}{\left(C{P}_k^{-}{C}^{\mathrm{T}}+R\right)}^{-\mathrm{l}}\text{(6.33)} \\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k\left({\mathbf{y}}_k-\mathbf{C}{\hat{\mathbf{x}}}_k^{-}\right)\text{(6.34)} \end{gathered}$$
我们再回过头来讨论一下卡尔曼增益的意义。在式(6.33)中，由于测量关系矩阵C和测量误差协方差矩阵R均假定为常系数阵，因而在第k历元的增益值K_k只是一个关于状态估计均方误差阵$P_k^{-}$的函数。

假设$P_k^{-}$值（先验状态量的误差）很大，大得使R值相对于$C{P}_k^{-}{C}^{\mathrm{T}}$而言趋向于零，那么增益K_k值就趋向于C^-1，相应地式(6.34)中的状态估计值${\hat{x}}_k$就基本上等于$C^{-1}{y}_k$。这就是说， 如果先验估计值${\hat{x}}_k^{-}$相对不可靠或者测量值y_k相对很准确，那么由式(6.33)计算得到的增益值会使状态估计${\hat{x}}_k$主倾向于信任y_k而减少对${\hat{x}}_k^{-}$的依赖，（也就是不相信状态/计算值 而相信测量值y_k）这自然相当地合情合理。

反过来，假如先验估计值${\hat{x}}_k^{-}$相对很可靠或者测量值y_k相对很粗糙，也就是说，$C{P}_k^{-}{C}^{\mathrm{T}}$值相对于R而言很小，那么增益K_k的值就趋向于零，相应地状态估计值${\hat{x}}_k$就倾向于信任${\hat{x}}_k^{-}$而减少对y_k的依赖，这同样十分合乎逻辑。

与a-β滤波器采用恒定滤波系数不同 ,卡尔曼滤波器能根据状态估计协方差与测量误差协方差的相对大小，自动、即时地调节增益值，以达到对系统状态的最优估计。

卡尔曼滤波器的最优运行，要求外界提供给滤波器的过程噪声方差Q和测量噪声方差R是准确的,否则卡尔曼增益和状态估计等参量不会达到它们的最优值,甚至还可能引起滤波器发散。例如，若输入给滤波器的过程噪声方差值Q严重偏小于其真实值，则卡尔曼增益值也将会严重偏小，致使测量值中所含的信息不能被及时、全部地反映在状态估计值上，最终会导致滤波器发散。对各个时刻的过程噪声和测量噪声方差值进行准确估算是卡尔曼滤波器设计、调试中重要而又困难的一步。

6.3.3 举例与讨论

与最小二乘法相比,上述卡尔曼滤波的计算过程体现了其递推算法的优越性:与最小二乘法需要等到收集完所有测量值后才能进行状态估算不同,卡尔曼滤波无需等到测量结束才能开始运行，而是每得到一个新的测量值，卡尔曼滤波就可以对系统状态估计值更新一次，并且前面所有测量值的信息都已经包含在当前的状态及其均方误差估计值中。在测量桌子长度这个例子中，我们或许没有什么时间紧迫感，等到测量全部结束再来计算桌子长度一般没什么不妥，然而对于定位、导航等一些实时性、动态性强的应用来说，我们必须在测量的同时进行实时滤波估算，此时卡尔曼滤波的递推算法就显得尤为重要。为了改进最小二乘法需要等到收集完所有测量值才能开始计算的这个缺点，于是递归最小二乘法(RLS)应运而生，它在形式上很接近于卡尔曼滤波。一般说来，递椎算法所需要的计算量相对较少。

同时一定要选择好初值，否则卡尔曼滤波估计值的精度会在很长时间内一直受到这些错误初始值的影响

对于α-β滤波器 和 卡尔曼滤波器

对于α-β滤波器
$$\begin{array}{rlr}& {\mathbf{x}}_k=\left[\begin{array}{cc}1& T_s\\ 0& 1\end{array}\right]{\mathbf{x}}_{k-1}+{\mathbf{w}}_{k-1}& (6.46\mathbf{A})\\ & y_k={\tilde{x}}_k=[1,0]{\mathbf{x}}_k+{\nu }_k& (6.46\mathbf{B})\end{array}$$
对于卡尔曼滤波器
$$\begin{array}{rlr}& {\mathbf{x}}_k=\left[\begin{array}{cc}1& T_s\\ 0& 1\end{array}\right]{\mathbf{x}}_{k-1}+{\mathbf{w}}_{k-1}& (6.46\mathbf{C})\\ & y_k={\tilde{x}}_k=K[1,0]{\mathbf{x}}_k+{\nu }_k& (6.46\mathbf{D})\end{array}$$
而这里的K就是
$$\left[\begin{array}{c}\alpha \\ \beta /T_s\end{array}\right]=K(6.46E)$$
则α-β滤波器就变成稳态形式的卡尔曼滤波器，从而使α-β滤波器取得最佳滤波效果。

6.3.4 滤波数值计算

问题

对于卡尔曼滤波而言

1.为什么计算机的有限位精度还可能造成状态均方误差阵Р在多次滤波更新后变得不再对称和正定。

2.P不再对称和正定对于卡尔曼滤波而言会有什么影响

3.为什么P一定要对称和正定

1. P是卡尔曼滤波的状态估计误差协方差矩阵，表示系统状态的估计精度。对于线性系统而言，P的更新遵循卡尔曼滤波的递推方程，并且P是一个对称的正定矩阵。

2. 计算机的有限位精度可能会对卡尔曼滤波的精度造成影响，导致状态均方误差阵Р在多次滤波更新后变得不再对称和正定。这是因为卡尔曼滤波器的计算过程中涉及到对各种矩阵的相乘、相加和逆运算等，这些运算可能会产生舍入误差，进而影响到卡尔曼滤波器的性能。

3. 当P不再对称和正定时，可能会导致卡尔曼滤波器的性能降低。例如，当P不再对称时，可能会导致状态估计值的偏移，从而影响到后续的控制计算和决策制定；当P不再正定时，可能会导致系统状态估计值不稳定，甚至无法有效地计算出状态估计值。因此，当出现这种情况时，需要及时采取措施来解决问题，例如重新校准传感器、重新调整滤波器参数等。

4. 首先，P对称的性质保证了对不同状态量之间的影响具有等价性，从而提高了状态估计值的精度和稳定性。

   其次，P一定要正定，主要是因为P正定的性质可以保证状态估计误差的协方差矩阵具有良好的几何意义，即保证状态估计误差的方向稳定（非奇异性）和大小可控。如果P不正定，可能会导致状态估计值不稳定，甚至无法有效地计算出状态估计值。

   举个例子来说，假设我们要利用卡尔曼滤波器进行车辆定位，测量包括车辆当前位置和速度等状态量，并根据这些状态量计算出下一个时刻的位置和速度等状态量。

   在卡尔曼滤波的计算中，**P的正定性可以确保状态估计误差的方向稳定性，即不会出现状态误差值长期偏离真实值的情况。**如果P不正定，可能会导致车辆位置和速度的状态估计值偏离真实值，影响到后续的控制计算和决策制定，从而导致整个定位系统的性能下降。

正题

对于当今发达的计算机技术来说，以式(6.31)~式(6.35)为代表的卡尔曼滤波递推算法相当简单，甚至相当优美。尽管如此，卡尔曼滤波算法的数值计算问题仍值得我们关注。此外，我们在这一小节还将继续探讨卡尔曼滤波器其他诸多方面的问题。

计算机通常用一个16位或32位的二进制数来表示一个数值,而这种由有限位数据在数值计算中所引起的量化误差可以视为一种噪声信号。为了补偿量化误差对滤k波计算的影响，我们在建立系统滤波模型时通常会特意增大滤波器的过程噪声。即使一个状态变量在理论上是个固定值，它在滤波模型中所对应的过程噪声量也不应该绝对等于零。

计算机的有限位精度还可能造成状态均方误差阵Р_k在多次滤波更新后变得不再对称和正定。 为了保k证Р_k的对称与正定，防止卡尔曼滤波器的发散，平方根卡尔曼滤波算法应运而生。顾名思义，平方根卡尔曼滤波算法的根本思想是将P_k分解成平方根形式，然后所有对P_k的操作实际上变成对P_k平方根阵的操作。 例如，一种相当流行的平方根卡尔曼滤波算法形式是将Р矩阵进行UD分解，即
$${\mathbf{P}}_k={\mathbf{U}}_k{\mathbf{D}}_k{\mathbf{U}}_k^{⊺}(6.47)$$
其中，U_k是一个对角线元素全为1的上三角矩阵，D_k是对角阵。这样，在卡尔曼滤波计算中所有对P的操作实际上成为对U_k和D_k的操作，并用U_k和D_k来计算卡尔曼增益K_k。

这种UD分解算法能自动保证P_k的对称与正定，卡尔曼滤波算法也变得较为稳定，但这一分解也使得卡尔曼滤波计算看上去复杂许多，计算量增大。在许多实际应用中，如果系统的各个状态变量具有一定的可观性，并且过程噪声又被建模得比较充分，那么我们其实不必过分担心卡尔曼滤波算法的不稳定性。

卡尔曼滤波器所选取的系统状态向量一般可有两种形式,即完整型状态向量x与状态校正向量Δx。

完整型状态向量包括位置、速度、钟差等参量，而相应的状态校正向量 包括位移、速度变化、钟差校正等参量，也就是说状态校正向量一般是完整型状态向量在两测量时刻之间的变化量。 通常，完整型状态变量的绝对值较大，而状态校正变量的绝对值较小。选用状态校正向量Δx作为卡尔曼滤波器系统状态向量的处理方法与上一章线性化后的最小二乘法相似，即Δx的估计值在每一历元的递推计算开x始前设置为零，而在计算结束时再将Δx的值加到相应的完整型状态向量x上。因此，若选用状态校正向量Δx，则滤波计算会处理一些值较小并且包含更多零的数据，这有利于减少运算量和计算误差。

标量测量法

**目的:**为解决矩阵求逆运算

大白话就是：把向量中的数值全部取出来，一个一个算，转换成标量计算，而不是矩阵计算。把矩阵中列向量一个个取出来，单独计算。

$$\begin{gathered} \begin{array}{rlr}{\hat{\mathbf{x}}}_k^{-}& =A{\hat{\mathbf{x}}}_{k-1}+B{\mathbf{u}}_{k-1}& (6.31)\\ {\mathbf{P}}_k^{-}& =A{\mathbf{P}}_{k-1}{A}^{\mathrm{T}}+\mathbf{Q}& (6.32)\end{array} \\ \begin{array}{c}{K}_k=P_k^{-}{C}^{\mathrm{T}}{\left(C{P}_k^{-}{C}^{\mathrm{T}}+R\right)}^{-\mathrm{l}}\text{(6.33)}\\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k\left({\mathbf{y}}_k-\mathbf{C}{\hat{\mathbf{x}}}_k^{-}\right)\text{(6.34)}\\ P_k=(I-K_{k}C)P_k^{-}\text{(6.35)}\end{array} \\ {y}_k=C{x}_k+{\nu }_k(6.11) \end{gathered}$$
优势：

不论是采用标量测量依次处理法还是采用原先的向量处理法，两者最后得到的结果${\hat{x}}_k$和$P_k$应该一致，但是由标量测量依次处理法所产生的各个卡尔曼增益向量k_m,k,一般不与由式(6.33)得到的增益矩阵K_k中的列向量相对应。在标量测量依次处理算法中，只要各个c_m，r_m与标量测量值y_m,k之间正确地一一相应，最后的测量校正结果就与各个标量测量值的先后排列顺序无关。

标量测量依次处理法的另一个优点在于其给接收机执行RAIM算法（见5.5节)提供了一个良好的机会。

在RAIM算法中，通过比较实际测量值和通过卡尔曼滤波得到的预测值，可以计算残差。如果某个测量值的残差超过了某个阈值，可能表明该测量值存在异常。这种逐个处理测量值的方法使得RAIM算法能够更灵活地检测和识别异常，而不会受到整体向量的影响。

如果用向量处理的话，就是需要所有测量更新计算之后，检测出某一个测量值是错误的，在删除C和R中对应于此错误的相关行和列，重复测量更新的所有运算。

直接通过标量处理，将向量的值一个个纳出来，一个个计算，而不用向量整体计算是否发生了差错，使得整个的运算效率得到了提升。

关于 测量噪声协方差阵R​

标量测量依次处理法假定了测量噪声协方差阵R为一个对角阵，但是如果各个测量噪声之间相关，那么R就不再是个对角阵。在这种情况下，为了使卡尔曼滤波的校正过程仍然能够进行标量测量依次处理，对进行正交分解是一个可行的解决方案。

关于上述这段话的解释

在标量测量依次处理法中，假定测量噪声协方差阵 (R) 为一个对角阵的前提是各个测量噪声之间是相互独立的。这使得卡尔曼滤波的校正过程变得简单，因为在这种情况下，卡尔曼滤波的校正步骤可以分别对待每个测量，并独立地进行校正。

然而，如果各个测量噪声之间存在相关性，即 (R) 不再是一个对角阵，那么这个简化就不再成立。在这种情况下，为了仍然能够使用标量测量依次处理法，可以采用正交分解的方法。

正交分解的思想是将相关的测量转化为一组相互独立的测量。通过对协方差阵 (R) 进行特征值分解或者正交变换，可以得到一个正交矩阵，使得新的测量值之间是相互独立的。这个正交矩阵的转置可以用于将原始的相关测量转化为相互独立的测量。

使用正交分解后，可以将卡尔曼滤波的校正步骤应用到新的独立测量上，然后再通过逆正交变换将结果映射回原始相关的测量空间。 这样，即使测量噪声之间存在相关性，也可以通过正交分解的方法来保持标量测量依次处理法的简化特性。这种方法的关键在于将相关的测量转化为相互独立的测量，以便在滤波过程中能够独立地处理每个测量。

卡尔曼滤波假定不同时刻的测量噪声之间互不相关，但卫星星历误差、大气延时和多路径等时常可能在一段时间内的测量值中引入一个共同的偏差。为了应付这种有色测量噪声的情况，我们可在系统状态向量中添加用来估算测量偏差的状态变量。 因为估计测量偏差值的大小不但不是我们的最终目标，而且它们的可观性一般也很差，所以我们又希望尽可能地去掉这些状态变量。施密特—卡尔曼滤波就是一种不在状态向量中添加测量偏差变量但又考虑它们对系统状态估计影响的方法,它对卡尔曼滤波递推计算公式进行了修改，但修改后的计算公式变得较为复杂。

关于上述的解释

当测量噪声存在相关性时，引入状态变量来估算测量偏差可以更好地应对这种情况。让我们考虑一个简化的例子，假设我们有一个单一维度的系统状态 (x)，并且我们测量这个状态，但测量中存在有色噪声。

系统动态方程可以表示为：

$x_{k+1}=A\cdot x_k+B\cdot u_k+w_k$

其中，(A) 和 (B) 是系统矩阵，$u_k$是输入 $w_k$是过程噪声。

测量方程可以表示为：

$z_k=H\cdot x_k+v_k$

其中，H 是测量矩阵，$v_k$是测量噪声。

在传统的卡尔曼滤波中，我们通常假设$w_k$ 和$v_k$是白噪声，即它们之间没有相关性。但实际上，在某些情况下，比如卫星导航系统中，不同时间点的测量可能受到相同的卫星钟差影响，导致测量噪声之间存在相关性。

为了处理这种情况，我们可以引入一个额外的状态变量，用来估计测量偏差。我们将这个额外的状态变量记为 $d_k$，其动态方程可以表示为：

$d_{k+1}=F\cdot d_k+u_k^{\prime }+q_k$

其中，(F) 是状态转移矩阵，$u_k^{\prime }$ 是输入，$q_k$ 是过程噪声。这个状态变量 $d_k$ 的存在允许我们对测量噪声中的偏差进行建模。

修改测量方程，考虑测量偏差：

$z_k=H\cdot x_k+d_k+v_k$

这样，我们的系统状态向量就包括了原始的系统状态 $x_k$ 和测量偏差 $d_k$。通过引入状态变量$d_k$，我们能够更灵活地处理有色测量噪声，并使卡尔曼滤波器能够适应实际系统中存在的相关性。这种方法可以在实际应用中提高卡尔曼滤波器对系统状态的估计准确性。

系统没有任何有效的测量值

关于测量值的另一个问题是，有时候系统没有任何有效测量值，比如GPS 接收机会遇到所有卫星信号被建筑物挡住的这种情况。卡尔曼滤波器可相当容易地处理这种没有测量值的情况.即照常进行状态及其均方误差的时间更新过程，并省略测量更新过程。然而，若系统缺少测量值的持续时间过久或者由此导致的状态估计均方误差过大，则我们应当否定滤波结果的有效性，甚至重置卡尔曼滤波器。 若某一时刻的测量值个数M小于状态变量个数N，则卡尔曼滤波的整个计算过程可照常进行，而这一特点可用来实现GPS接收机的一星、二星定位，提高定位有效率。类似于5.3.3节所介绍的在最小二乘法中添加辅助方程式的做法，我们同样可以在卡尔曼滤波中添加辅助方程式，每个辅助方程式相当于一个标量测量值。

6.3.5 非线性滤波

卡尔曼最初提出的滤波理论只适合线性测量方程的线性控制系统，但是实际上我们现实中碰到的问题几乎是非线性的。

一个离散时间的非线性系统以及其非线性测量量可以用下面的两个方程式来描述：
$$\begin{array}{rlr}{x}_k& =f(x_{k-1},u_{k-1},w_{k-1})& (6.64)\\ y_k& =h(x_k,v_k)& (6.65)\end{array}$$

其中，$f=[f_1{f}_2\cdots f_N{]}^{⊺}$和$h=[h_1{h}_2\cdots h_M{]}^{⊺}$均为一列非线性函数向量

于是就有 ➡️ 扩展卡尔曼滤波EKF 和线性化卡尔曼滤波

扩展卡尔曼滤波EKF

基本思路: 假定卡尔曼滤波**对当前系统状态的估计值非常接近于其真实值，于是将非线性函数f与h在当前状态估计值处进行台劳展开并实现线性化。**也就是在${\hat{x}}_k$处进行泰勒展开 使其线性化

对于EKF的推导大家可以看看b站的这个视频

【【卡尔曼滤波器】6_扩展卡尔曼滤波器_Extended Kalman Filter】 https://www.bilibili.com/video/BV1jK4y1U78V/?share_source=copy_web&vd_source=8d5f94cac4ef1f7256e3572189ec255b

$$\begin{array}{rlr}{x}_k& =f(x_{k-1},u_{k-1},w_{k-1})& (6.64)\\ y_k& =h(x_k,v_k)& (6.65)\end{array}$$
对于上式（6.67）的线性化，其实就是（6.64）在${\hat{x}}_k$处的泰勒展开式。 也印证了我们的基本思路 : 假定卡尔曼滤波 对当前系统状态的估计值非常接近于其真实值，于是将非线性函数f与h在当前状态估计值处进行台劳展开并实现线性化。
$$f(x)=f(x_0)+\frac{f^{\prime }(x_0)}{1!}(x-x_0)+\frac{f^{\prime \prime }(x_0)}{2!}(x-x_0{)}^2+\cdots +\frac{f^{(n)}(x_0)}{n!}(x-x_0{)}^n+R_n$$
只不过是在${\hat{x}}_k$处的一阶展开

同时（6.71）式到 （6.72）的推导过程用下面的结论就可以出来~

⭐️结论：(直接拿来应用就好)

若$P(x)\sim N(0,Q)$那么可以推导出

$P(xw)\sim N(0,wQ{w}^T)$

关于雅可比矩阵​

雅可比矩阵在微积分中用于表示多元函数的导数。 在线性系统中，状态转移函数和测量函数通常是线性的，因此雅可比矩阵简化了滤波算法的推导和实现。在非线性系统中，通过在当前状态点处对非线性函数进行线性化，雅可比矩阵使得卡尔曼滤波算法仍然可以应用。

具体来说，对于一个非线性状态转移函数 $f(x_k,u_k)$和非线性测量函数 $h(x_k)$，在卡尔曼滤波的预测和更新步骤中，需要使用这些函数在当前状态估计点处的导数。

卡尔曼滤波的预测步骤中，需要计算状态转移函数$f$ 在当前状态估计点${\hat{x}}_k$处的雅可比矩阵$A_k$。这个雅可比矩阵代表了状态转移函数在当前状态点处的线性近似，使得非线性系统的预测步骤可以使用与线性系统相似的方法进行。

卡尔曼滤波的更新步骤中，需要计算测量函数 $h$在预测状态估计点${\hat{x}}_{k+1}^{-}$ 处的雅可比矩阵$H_k$。这个雅可比矩阵表示了测量函数在当前预测状态点处的线性近似，同样使得非线性系统的更新步骤可以使用与线性系统相似的方法进行。

通过使用雅可比矩阵，可以将非线性系统的状态估计问题转化为一系列线性系统的问题，从而使用卡尔曼滤波的线性形式来处理。这是卡尔曼滤波及其扩展形式（如扩展卡尔曼滤波）在处理非线性问题时的关键之一。

$$\begin{array}{rlr}{x}_k& =f(x_{k-1},u_{k-1},w_{k-1})& (6.64)\\ y_k& =h(x_k,v_k)& (6.65)\end{array}$$

总结

⭐️整个的过程如下：​

举个栗子

非线性卡尔曼滤波

扩展卡尔曼滤波：将k非线性函数f与h在滤波器对当前系统状态x_k的最优估计值处线性化

非线性卡尔曼滤波：因为预先知道非线性系统的实际运行状态x_k大致按照所要求、希望的轨迹${\overline{x}}_k$变化，所以这些非线性函数在x_k点的值可以表达为在${\overline{x}}_k$处的台劳展开式，从而完成线性化。

也就是非线性卡尔曼滤波需要预先知道系统运动状态的轨迹。但是对于定位导航系统来说，因为用户的运动情况不可预测，所以GPS接收机采用的EKF算法

优点： 它的非线性函数f与h在各个台劳展开点${\overline{x}}_k$处的偏导值可以是预先算好的，这可以减少系统在实时运行时的计算量。 此外，如果扩展卡尔曼滤波对系统状态的估计发生严重偏差,那么非线性函数在此错误的状态估计值处线性化可导致扩展卡尔曼滤波性能降低，甚至导致滤波器发散，因而从这一方面来讲，线性化卡尔曼滤波显得相对保守与稳定。

考虑到扩展卡尔曼滤波通常可得到相当准确的状态估计值，它的滤波性能事实上要好于线性化卡尔曼滤波。在解决一些实际滤波问题的各种滤波技术中，扩展卡尔曼滤波已经成为应用最广泛的一种。

关于系统的噪声问题​

卡尔曼滤波需要假定系统的过程噪声与测量噪声均为正态白噪声,但是一个正态白噪声经过非线性变换后通常不再呈正态白噪声,这就破坏了扩展卡尔曼滤波与线性化卡尔曼滤波的前提假设。

因此，尽管卡尔曼滤波器在所有线性滤波器中是最优的一个，但是在处理非线性系统或者非正态白噪声问题时，卡尔曼滤波器不再是一个最优滤波器，而一个非线性滤波器的滤波性能可能会更好。

6.4 系统模型的建立

现实中的系统控制过程很多表现为连续时间型

此讲需要涉及信号与系统相关的知识

6.4.1 连续时间系统的建模

⭐️我们的目标：

将系统函数H(s)表示的一个N阶微分方程式等价地变成由N个一阶微分方程式组成的矩阵方程式(6.89)和一个离散时间型测量方程式(6.92)。这种变换有利于我们进行系统分析与实现，而变换后所得的式(6.89)和式(6.92)一起通常称为系统的状态空间模型。

也就是对于一个单输入，单输出的LTI系统
$$H(s)=\frac{Y(s)}{U(s)}=\frac{a_1{s}^{N-1}+a_2{s}^{N-2}}{s^N+b_1{s}^{N-1}}\frac{+\cdots +a_N}{+\cdots +b_N}(6.82)$$
对于U(s)：

对于Y(s)：

至此，我们给出如下一个用于描述连续时间系统的通用型状态空间模型:
$$\begin{array}{rlr}\dot{\mathbf{x}}(t)& =\mathbf{F}\mathbf{x}(t)+\mathbf{G}\mathbf{e}(t)& (6.93)\\ y_k& ={\mathbf{C}}_k{\mathbf{x}}_k+{\mathbf{v}}_k& (6.94)\end{array}$$
卡尔曼滤波认为噪声向量e(t)中的各个分量之间相互独立，并且每个分量均是一个高斯白噪声，因而e(t)
的自相关函数具有以下形式:
$$R_e\left(\tau \right)=E\left[e(t+\tau )e^T\left(\tau \right)\right]=W\delta (\tau )（6.95）$$
其中δ(t)为冲激函数，而W为对角阵，并且对角线上的各个元素值等于相应噪声分量的功率谱密度的幅值。为了进一步简化公式推导，我们假定F与G均为常系数矩阵。

通用型状态空间模型可非常贴切地用来描述GPS接收机的工作方式**:用户接收机的运动状态（位置、速度等）随时间的变化可用一个微分方程式来表达**，而接收机每隔一定时间获取与其当前运动状态有关的伪距、载波相位等离散型卫星信号测量值。

接下来的公式推导任务是将状态空间模型中的连续时间型微分方程式(6.93)转换成离散型卡尔曼滤波的状态方程式(6.10)，而这种转换并不是通过简单、直接的采样离散来完成的。我们在这里所采用的方法是1️⃣将此微分方程式进行拉氏变换,从而在复频域中解出系统状态X(s),2️⃣然后再将X(s)进行拉氏反变换到时域中的原函数x(t)，3️⃣最后对连续时间型函数x(t)进行周期采样而达到离散的目的。同时,这一离散方法对于过程噪声协方差值大小的选取也具有指导意义。

最终推导出：
$$\begin{array}{rlr}{x}_k& =A_{k-1}{x}_{k-1}+w_{k-1}& (6.105)\\ y_k& =C_k{x}_k+{\nu }_k& (6.106)\end{array}$$
这与如式(6.10)和式(6.11)所描述的卡尔曼滤波模型相一致。

6.4.2 离散时间系统的建模

和上述连续时间系统建模类似，而且更加简单。只不过把t改成了n

6.5 GPS定位的卡尔曼滤波算法

运用卡尔曼滤波算法求解GPS位置 速度 和时间（PVT）

我们知道，卡尔曼滤波与第5章中介绍的最小二乘法的主要区别在于:

卡尔曼滤波：利用状态方程将不同时刻的系统状态联系起来

最小二乘法：孤立地求解每一个不同时刻的系统状态。

除此之外，GPS 的卡尔曼滤波测量方程与最小二乘法中的定位、定速方程事实上没有差别,**因而状态变量与状态方程的选取在很大程度上决定了GPS 卡尔曼滤波算法的特点和性能。**鉴于此，本节就将注意力集中在GPS卡尔曼滤波状态方程的建立上。

6.5.1 接收机时钟模型

接收机时钟偏差是用于GPS定位的各个卫星伪距测量值的公共误差部分，而接收机时钟频漂是用于GPS定速的各个卫星多普勒频移测量值的公共误差部分。因为接收机的钟差与频漂之间是一个简单的积分关系，所以它们两者经常一起作为卡尔曼滤波的系统状态变量。

下面具体说明：

6.5.2用户运动模型

不同特点的用户运动形式可用不同的状态方程模型来描述。运动状态方程既要尽可能地真实、完整地描述用户的运动规律，又要控制状态向量的维数，以减少计算量。
对于静态接收机而言，因为它的运动速度为零，所以由用户位置的三个坐标分量(x,y,z)和接收机的两个时钟变量(δt_u ,δf_u)所组成的一个五维状态向量x足以描述该接收机的运行状态。在形式上如同式(6.105)的状态方程中，状态向量为
$$\mathbf{x}={\left[\begin{array}{ccccc}x& y& z& \delta t_u& \delta f_u\end{array}\right]}^{\mathrm{T}}(6.119)$$

6.5.3 卡尔曼滤波定位算法

我们在以上两小节选定了系统的状态向量，并且又建立了卡尔曼滤波状态方程，而这一小节的任务是建立卡尔曼滤波测量方程。

GPS接收机的测量值：伪距和多普勒频移（或积分多普勒）

我们要做：建立测量值和前面已经选定状态变量之间的关系

卡尔曼滤波测量方程式：

伪距观测方程式 （接收机空间位置和式子钟差状态变量的可观性）

多普勒频移观测方程式（若状态向量中还包括速度什么的）

经过必要的线性化 得到卡尔曼滤波测量方程

对于测量误差方差的确定：

我们可以根据卫星导航电文中的URA值卫星仰角、卫星信号强度、接收机信号跟踪状况和当时当地的环境情况等各种可能获知的信息而一一估算出各项误差量的方差，然后将综合出的测量误差方差交给接收机的定位运算。在实践中，我们时常也可以通过实验来测定测量误差方差，然后建立一个关于信号信噪比或者卫星仰角的测量误差方差函数。

进行卡尔曼滤波递推计算

1️⃣在每一定位历元，卡尔曼滤波器首先利用状态方程预测接收机当前的位置、速度和钟差等状态;2️⃣根据这一状态先验估计值以及卫星星历所提供的卫星位置与速度卡尔曼滤波器就可以预测GPS 接收机对各颗卫星的伪距与多普勒频移值,而这些测量预测值与接收机的实际测量值之间的差异形成测量残余3️⃣ 最后，卡尔曼滤波的校正过程通过处理测量残余而得到系统状态估计值的校正量及其校正后的最优估计值。

如果多个时刻有多个卫星测量残余的绝对值过大，那么很有可能是卡尔曼滤波的状态估计值出现严重偏差，此时系统状态与滤波器―般需要重置。

6.6 其他滤波技术

卡尔曼滤波是一种有着相当广泛应用的滤波方法，但它既需要假定系统是线性的，又需要认为系统中的各个噪声与状态变量均呈高斯分布,而这两条并不总是确切的假设限制了卡尔曼滤波器在现实生活中的应用。

虽然扩展卡尔曼滤波将非线性系统线性化,但只有当非线性函数在台劳展开点附近呈相对线性的情况下，该算法才比较稳定。对于某些非线性系统来说，计算线性化过程中的雅可比矩阵并不是一件简单的事情。 此外，扩展卡尔曼滤波器时常给人的感觉是难以调节其有关的滤波参数，在实际中应用起来并不容易。

不敏卡尔曼滤波器UKF

针对非线性系统改进下卡尔曼滤波器

不敏变换 描述高斯随机变量在非线性变换后的概率分布情况的方法

不敏卡尔曼滤波认为，与其将一个非线性变换(或函数）线性化、近似化，还不如将高斯随机变量经非线性变换后的概率分布情况用高斯分布来近似那样简单，因而不敏卡尔曼滤波算法没有非线性化这一步骤。

在每一定位历元，不敏卡尔曼滤波器按照一套公式产生一系列样点，每一样点均配有一个相应的权重，而这些带权的样点被用来完整地描述系统状态向量估计值的分布情况,它们替代了原先卡尔曼滤波器中的状态向量估计值及其协方差。不敏卡尔曼滤波器让这些样点一一经历非线性状态方程与测量方程，然后再将这些经非线性变换后的样点按照它们的权重而综合出对当前时刻的系统状态向量估计值。 与扩展卡尔曼滤波器相比，不敏卡尔曼滤波―般能提供更为准确的滤波定位结果，然而它需要更多的计算量。

扩展卡尔曼滤波与不敏卡尔曼滤波在一定程度上可以解决-些非线性系统的滤波问题,但对于强非线性、非高斯系统，卡尔曼滤波器一般会变得难以胜任。

粒子滤波器

粒子( Particle)滤波器是一种适用于强非线性、无高斯假设的统计滤波器，它不是对系统模型做包括线性处理在内的任何变化，而是在统计上、数值上得到系统最优估计的近似解

稍具体一点地说，粒子滤波器将系统状态变量的后验概率密度函数用一些随机样本即“粒子”来代表，并且给每个粒子赋予一个相应的权重。 这种粒子与不敏卡尔曼滤波器中的样点有些相似，但两者也有明显的区别: 1️⃣首先，不敏卡尔曼滤波器的样点及其权重是按照一定公式产生的，而粒子则是随机的;2️⃣其次，为了完整地描述一个非高斯分布，粒子的数量通常要比样点数量多一两个级数。3️⃣在每一历元，粒子滤波器首先根据状态方程计算出各个粒子的预测值，然后根据测量残余来更新各个粒子的权重，而系统状态估计值等于所有粒子的加权和，最后再对粒子进行重采样。

粒子滤波器算法目前所面临的一个主要问题是重采样步骤带来的粒子枯竭，但同时我们在实际应用中又希望限制粒子数目，以尽可能地控制计算量增加的程度。

多态自适应卡尔曼滤波器MMA

多态自适应（MMA)卡尔曼滤波器是一种受到广泛关注的滤波器，它由好多个并联、同时运行的卡尔曼滤波器组成。

在这组卡尔曼滤波器中，每一个滤波器对未知的滤波参数分别做出相互不同的假设，然后各自按照自己的模型假设进行滤波计算，而多态自适应滤波器最后将它们对系统状态的各个估计值进行加权，并以此作为最优估计值输出 。 因为每个卡尔曼滤波器各自有着不同值的滤波参数，所以我们希望它们中的某一个总能比较准确地描述系统在某一时段、环境下的实际运行状况。如果是这样，那么该卡尔曼滤波器―般应该表现为产生较小的测量残余，而它的权重在整组滤波器就会自动地得到提升。

交互式多模型——概率数据关联滤波器（IMM-PDAF)是另一种备受关注的多态滤波形式，它对一运动物体系统建立多个不同的运动模型，认为系统总是在这些不同的模型之间切换，并让每一种运动模型与一个有适当参数值的概率数据关联滤波器相对应,它对于跟踪在密集杂波情况下的高机动目标具有良好的滤波效果。

啊，这章终于结束了！
目前不打算更后面的内容和章节了~ 但是遇到我认为比较重要的内容会继续分享的~
☘️>ok,完结~(●’◡’●) 看到这里 点个赞叭 (●’◡’●)