文章目录

  • 李永乐老师讲卡尔曼的一张截图
  • 卡尔曼滤波器
    • 置信度位高斯分布的贝叶斯滤波器
  • 卡尔曼推导（可以跳过）
    • 1.0定义
    • 2.0文献
    • 3.0基本动态系统模型
    • 3.1两个模型——状态转移和观测:
    • 3.2纯粹的时域滤波器
    • 3.3卡尔曼滤波器的状态
    • 3.4直接点说，只要能写出B，Q，R，H和F矩阵的动态系统，都能用卡尔曼滤波解决动态状态估计问题。
    • 3.5 过程一：预测
    • 3.6过程二：更新
      • 总结更新过程：
      • 3.6.1 最优卡尔曼增益
    • 3.7不变量（Invariant）
      • 参考reference：
  • 传统 KF 分析小车运动（不是多源融合）
    • 1、运动与观测
    • 2、对于状态方程中的系统噪声w和测量噪声v，假设服从如下多元高斯分布，并且w,v是相互独立的。其中Q,R为噪声变量的协方差矩阵。
    • 3、初值PKP_KPK​
    • 协方差矩阵的计算(可以了解)
    • （四）总结
    • 附上某bilibili课件图片
    • CRTV等汽车运动模型
    • 例子
  • 多元高斯分布协方差矩阵
  • 拓展卡尔曼
    • 关于泰勒展开的故事：
      • 泰勒展开，初值对估计的影响
      • 推导过程（可以忽略）
    • 言归正传：
    • EKF的线性化采用一阶泰勒展开
    • 实际上python中有专门计算梯度和雅克比矩阵的库numdifftools
    • 过程总结
    • EKF-近似最优卡尔曼增益
  • 状态向量的选取经验（可以略过）
  • EKF 例子
  • 另一种EKF_error state KF
  • 多传感器融合-lidar+Radar
    • 初始化
    • 预测
    • 修正
      • lidar使用了KF
      • radar使用了EKF
      • 传感器融合实例

李永乐老师讲卡尔曼的一张截图

卡尔曼滤波器

卡尔曼滤波器是在估计线性系统状态的过程中，以最小均方误差为目的而推导出的几个递推数学等式。

也可以从贝叶斯推断的角度来推导，如下图。

置信度位高斯分布的贝叶斯滤波器

最可能状态：均值
不确定度：方差

高斯分布





状态转移左乘AAA
协方差矩阵转呀 AΣATAΣA^TAΣAT


协方差在概率论和统计学中用于衡量两个变量的总体误差，其实方差就是协方差的一种特殊情况，可以理解为两个相同变量的协方差


如何融合两个高斯分布？

对于两个相互独立的高斯分布，我们将他们的概率密度相乘


卡尔曼推导（可以跳过）

1.0定义

卡尔曼滤波是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。这个估计可以是对当前目标位置的估计（滤波），也可以是对于将来位置的估计（预测），也可以是对过去位置的估计（插值或平滑）。——维基百科

2.0文献

http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf

3.0基本动态系统模型

基本动态系统可以用一个马尔可夫链表示，该马尔可夫链建立在一个被高斯噪声（即正态分布的噪声）干扰的线性算子上的[1]。

卡尔曼滤波器的模型。圆圈代表向量，方块代表矩阵，星号代表高斯噪声，其协方差矩阵在右下方标出。

从一系列有噪声的观察数据中用卡尔曼滤波器估计出被观察过程的内部状态.对于每一步k，定义矩阵Fk,Hk,Qk,RkF_k, H_k, Q_k,R_kFk​,Hk​,Qk​,Rk​，有时也需要定义BkB_kBk​。

And,
FkF_kFk​是作用在xk−1x_{k−1}xk−1​上的状态变换模型（/矩阵/矢量）。
BkB_kBk​是作用在控制器向量uku_kuk​上的输入－控制模型。
wkw_kwk​是过程噪声，并假定其符合均值为零，协方差矩阵为QkQ_kQk​的多元正态分布。WkN(0,Qk)W_k~N(0,Q_k)Wk​ N(0,Qk​).
HkH_kHk​是观测模型，它把真实状态空间映射成观测空间，vkv_kvk​是观测噪声，其均值为零，协方差矩阵为RkR_kRk​,且服从正态分布。VkN(0,Rk)V_k~N(0,R_k)Vk​ N(0,Rk​)

3.1两个模型——状态转移和观测:

假设kkk时刻的真实状态是从（k−1）（k − 1）（k−1）时刻的状态演化,[Control Model]
xk=Fkxk−1+Bkuk−1+wk−1x_k=F_kx_{k-1}+B_ku_{k-1}+w_{k-1}xk​=Fk​xk−1​+Bk​uk−1​+wk−1​

kkk时刻，对真实状态xkx_kxk​的一个测量zkz_kzk​满足下式：[Observation Model]
zk=Hkxk+vkz_k=H_kx_k+v_kzk​=Hk​xk​+vk​

3.2纯粹的时域滤波器

卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值， 因此不需要记录观测或者估计的历史信息。卡尔曼滤波器与大多数滤波器不同之處，在於它是一种纯粹的时域滤波器，它不需要像低通滤波器等频域滤波器那样，需要在频域设计再转换到时域实现。

3.3卡尔曼滤波器的状态

卡尔曼滤波器的状态由以下两个变量表示:
x^k∣k\hat{x}_{k|k}x^k∣k​:在时刻k的状态的估计；
Pk∣kP_{k|k}Pk∣k​:误差相关矩阵，度量估计值的精确程度。

3.4直接点说，只要能写出B，Q，R，H和F矩阵的动态系统，都能用卡尔曼滤波解决动态状态估计问题。

3.5 过程一：预测

卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。
1.(预测状态)
x^k∣k−1=Fkx^k−1∣k−1+Bkuk\hat{x}_{k|k-1}=F_{k}\hat{x}_{k-1|k-1}+B_ku_{k}x^k∣k−1​=Fk​x^k−1∣k−1​+Bk​uk​

2.(预测估计协方差矩阵)
Pk∣k−1=FkPk−1∣k−1FkT+QkP_{k|k-1}=F_kP_{k-1|k-1}F_k^{T}+Q_kPk∣k−1​=Fk​Pk−1∣k−1​FkT​+Qk​

3.6过程二：更新

首先要算出以下三个量：

y~k=zk−Hkx^k∣k−1\tilde{\textbf{y}}_{k} = \textbf{z}_{k} - \textbf{H}_{k}\hat{\textbf{x}}_{k|k-1}y~​k​=zk​−Hk​x^k∣k−1​(测量余量，measurement residual)
Sk=HkPk∣k−1HkT+Rk\textbf{S}_{k} = \textbf{H}_{k}\textbf{P}_{k|k-1}\textbf{H}_{k}^{T} + \textbf{R}_{k}Sk​=Hk​Pk∣k−1​HkT​+Rk​(测量余量协方差)
Kk=Pk∣k−1HkTSk−1\textbf{K}_{k} = \textbf{P}_{k|k-1}\textbf{H}_{k}^{T}\textbf{S}_{k}^{-1}Kk​=Pk∣k−1​HkT​Sk−1​(最优卡尔曼增益）

然后用它们来更新滤波器变量Xk,PkX_k ,P_kXk​,Pk​：

x^k∣k=x^k∣k−1+Kky~k\hat{\textbf{x}}_{k|k} = \hat{\textbf{x}}_{k|k-1} + \textbf{K}_{k}\tilde{\textbf{y}}_{k}x^k∣k​=x^k∣k−1​+Kk​y~​k​(更新的状态估计)
Pk∣k=(I−KkHk)Pk∣k−1\textbf{P}_{k|k} =(I - \textbf{K}_{k} \textbf{H}_{k}) \textbf{P}_{k|k-1}Pk∣k​=(I−Kk​Hk​)Pk∣k−1​(更新的协方差估计）
使用上述公式计算Pk∣k\textbf{P}_{k|k}Pk∣k​仅在最优卡尔曼增益的时候有效。使用其他增益的话，公式要复杂一些。

高斯分布经过线性变换后还是高斯分布
高斯分布经过非线性变换后还是非高斯分布








总结更新过程：

x^k∣k=x^k∣k−1+Kky~k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)=(1−KkHk)x^k∣k−1+Kkzk\hat{{x}}_{k|k} = \hat{{x}}_{k|k-1} + {K}_{k}\tilde{{y}}_{k}=\hat{{x}}_{k|k-1} + {K}_{k}({z}_{k} - {H}_{k}\hat{{x}}_{k|k-1})=(1- {K}_{k}{H}_{k})\hat{{x}}_{k|k-1}+{K}_{k}{z}_{k}x^k∣k​=x^k∣k−1​+Kk​y~​k​=x^k∣k−1​+Kk​(zk​−Hk​x^k∣k−1​)=(1−Kk​Hk​)x^k∣k−1​+Kk​zk​

x^k∣k−1\hat{{x}}_{k|k-1}x^k∣k−1​是基于k-1时刻的最优估计和模型（先验分布）得到的预测值，zk{z}_{k}zk​是k时刻的观测值，也就是说，我们的最优估计实际上是对测量值和预测值的一个加权平均，而 Pk∣k{P}_{k|k}Pk∣k​就是迭代过程中的最优加权系数（实际上是根据方差确定比例）。

3.6.1 最优卡尔曼增益

  • https://blog.csdn.net/u013102281/article/details/59109566?depth_1-utm_source=distribute.pc_relevant.none-task&utm_source=distribute.pc_relevant.none-task

最优卡尔曼增益,Pk∣k=(I−KkHk)Pk∣k−1\textbf{P}_{k|k} =(I - \textbf{K}_{k} \textbf{H}_{k}) \textbf{P}_{k|k-1}Pk∣k​=(I−Kk​Hk​)Pk∣k−1​(更新的协方差估计）这个公式的计算比较简单，所以实际中总是使用这个公式，但是需注意这公式仅在使用最优卡尔曼增益的时候它才成立。如果算术精度总是很低而导致数值稳定性出现问题，或者特意使用非最优卡尔曼增益，那么就不能使用这个简化；必须使用导出的后验误差协方差公式。

此式可以写作

Pk∣k=(I−KkHk)cov(xk−x^k∣k−1,I−KkHk)T+Kkcov(vk)KkT\textbf{P}_{k|k} =(I - \textbf{K}_k \textbf{H}_{k})\textrm{cov}(\textbf{x}_k - \hat{\textbf{x}}_{k|k-1},I - \textbf{K}_k \textbf{H}_{k})^{T} + \textbf{K}_k\textrm{cov}(\textbf{v}_k )\textbf{K}_k^{T}Pk∣k​=(I−Kk​Hk​)cov(xk​−x^k∣k−1​,I−Kk​Hk​)T+Kk​cov(vk​)KkT​

使用不变量Pk∣k−1P_{k|k-1}Pk∣k−1​以及RkR_kRk​的定义这一项可以写作 ：
Pk∣k=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkT\textbf{P}_{k|k}=(I - \textbf{K}_k \textbf{H}_{k}) \textbf{P}_{k|k-1}(I - \textbf{K}_k \textbf{H}_{k})^T +\textbf{K}_k \textbf{R}_k \textbf{K}_k^TPk∣k​=(I−Kk​Hk​)Pk∣k−1​(I−Kk​Hk​)T+Kk​Rk​KkT​ 这一公式对于任何卡尔曼增益KkK_kKk​都成立。

参考：https://blog.csdn.net/DinnerHowe/article/details/79483194 推导Pk∣kP_{k|k}Pk∣k​的过程。

3.7不变量（Invariant）

如果模型准确，而且x^0∣0\hat{\textbf{x}}_{0|0}x^0∣0​与P0∣0\textbf{P}_{0|0}P0∣0​的值准确的反映了最初状态的分布，那么以下不变量就保持不变：所有估计的误差均值为零

E[xk−x^k∣k]=E[xk−x^k∣k−1]=0\textrm{E}[\textbf{x}_k - \hat{\textbf{x}}_{k|k}] = \textrm{E}[\textbf{x}_k - \hat{\textbf{x}}_{k|k-1}] = 0E[xk​−x^k∣k​]=E[xk​−x^k∣k−1​]=0
E[y~k]=0\textrm{E}[\tilde{\textbf{y}}_k] = 0E[y~​k​]=0
且协方差矩阵准确的反映了估计的协方差：

Pk∣k=cov(xk−x^k∣k)\textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_k - \hat{\textbf{x}}_{k|k})Pk∣k​=cov(xk​−x^k∣k​)
Pk∣k−1=cov(xk−x^k∣k−1)\textbf{P}_{k|k-1} = \textrm{cov}(\textbf{x}_k - \hat{\textbf{x}}_{k|k-1})Pk∣k−1​=cov(xk​−x^k∣k−1​)
Sk=cov(y~k)\textbf{S}_{k} = \textrm{cov}(\tilde{\textbf{y}}_k)Sk​=cov(y~​k​)
请注意，其中E[a]\textrm{E}[\textbf{a}]E[a]表示a{a}a的期望值,
cov(a)=E[aaT]\textrm{cov}(\textbf{a}) = \textrm{E}[\textbf{a}\textbf{a}^T]cov(a)=E[aaT]。

参考reference：

[1]卡尔曼滤波算法原理以及python实例https://blog.csdn.net/DinnerHowe/article/details/79483194
[2]https://blog.csdn.net/banzhuan133/article/details/103570744


图源：https://blog.csdn.net/u013102281/article/details/59109566?depth_1-utm_source=distribute.pc_relevant.none-task&utm_source=distribute.pc_relevant.none-task

传统 KF 分析小车运动（不是多源融合）

有一个匀加速运动的小车，它受到的合力为 ft , 由匀加速运动的位移和速度公式，能得到由 t-1 到 t 时刻的位移和速度变化公式：

1、运动与观测

该系统系统的状态向量包括位移和速度，分别用 xt 和 xt的导数 表示。控制输入变量为u，也就是加速度，于是有如下形式:

xk=Fkxk−1+Bkuk−1+wk−1x_k=F_kx_{k-1}+B_ku_{k-1}+w_{k-1}xk​=Fk​xk−1​+Bk​uk−1​+wk−1​

这里对应的的矩阵F大小为 2x2 ,矩阵B大小为 2x1。

用超声波仪器测量小车离我们的距离 :

zk=Hkxk+vkz_k=H_kx_k+v_kzk​=Hk​xk​+vk​

2、对于状态方程中的系统噪声w和测量噪声v，假设服从如下多元高斯分布，并且w,v是相互独立的。其中Q,R为噪声变量的协方差矩阵。

wk——N(0,Qk)w_k——N(0,Q_k)wk​——N(0,Qk​).
vk——N(0,Rk)v_k——N(0,R_k)vk​——N(0,Rk​)

对于小车匀加速运动的的模型，假设系统的噪声向量只存在速度分量上，且速度噪声的方差是一个常量0.01，位移分量上的系统噪声为0。Q中，叠加在速度上系统噪声方差为0.01，位移上的为0，它们间协方差为0，即噪声间没有关联。

测量值只有位移，它的协方差矩阵大小是1*1，就是测量噪声的方差本身。
R=[0.1]1×1R=[0.1]_{1×1}R=[0.1]1×1​

3、初值PKP_KPK​

估计值和真实值间误差的协方差矩阵 Pk−1∣k−1P_{k-1|k-1}Pk−1∣k−1​，协方差矩阵的对角线元素就是方差，求这个协方差矩阵，就是为了利用他的对角线元素的和计算得到均方差.

如果是匀加速运动模型里，ek便是由位移误差和速度误差，他们组成的协方差矩阵。

其中，Serr代表位移误差，Verr代表速度误差,对角线上就是各自的方差。

更新 PKP_KPK​

Pk∣k−1=FkPk−1∣k−1FkT+QkP_{k|k-1}=F_kP_{k-1|k-1}F_k^{T}+Q_kPk∣k−1​=Fk​Pk−1∣k−1​FkT​+Qk​

协方差矩阵的计算(可以了解)

  • https://blog.csdn.net/u011362822/article/details/95905113

在整个kalmam滤波的操作过程中，有3个协方差矩阵是需要特殊注意的，也是很多人使用时不知如何设置和更新的，分别是状态协方差矩阵P，过程噪声协方差矩阵Q，测量噪声协方差矩阵R。

（一）状态协方差矩阵P

状态协方差矩阵P就是状态之间的协方差组成的矩阵，对角线元素是各个状态的方差，其余元素是相应元素的协方差，由此也能看出P是一个多维方阵，维度和状态数一致，且P是对称方阵。

比如状态X包含位置p和速度v两个量，则对应的协方差矩阵如下式所示：

由于相同变量之间的协方差就是其方差，因此对角线上元素分别是p和v的方差，其余两个元素分别是两元素间的协方差，由于协方差部分次序，协方差矩阵式对称的。

在使用时协方差矩阵P是一个迭代更新的量，每一轮预测和更新后，P都会更新一个新的值，因此初始化时可以根据估计协定，不用太苛求初始化的精准度，因为随着几轮迭代会越来越趋紧真实值。

（二）过程误差协方差矩阵Q

该矩阵的每一个元素分别是状态X的元素误差之间的协方差，以上文中的状态X为例，其包含位置p和速度v两个元素，其误差状态为：

其Q矩阵可用下式表示：

该矩阵是由不确定的噪声引起的，确定Q的各元素大小是不容易的，使用时都是具体问题具体分析。

比方说针对上文中的小车的状态，误差来源是移动中的打滑等，最底层的是由于力的变化导致的加速度的变化，因此我们找到了加速度方差，就可以推导出Q矩阵，假设加速度方差是D(a)。

根据，

有，

因此，

（三）测量噪声协方差矩阵R

测量噪声协方差的来源是传感器误差，即传感器的不准确性，在使用时一般传感器都会给出精度指标，该指标就可以直接转化到矩阵R中。

还以上文小车为例，假设测量矩阵为下式所示：

表示状态X中的位置p和速度v均可以测量，观测误差协方差矩阵R用下式表示：

此时对角线上就是各测量量的测量方差，从传感器说明书上均可以读到，另外针对状态之间的协方差吐过确定不了就可以设为0，即可以如下设置：

（四）总结

P矩阵可以根据状态方差估计，不用太最求准确度，因为P矩阵会随着迭代次数的增加而收敛到准确值；
Q矩阵是状态转移预测过程由于外部干扰产生，移动机器人中可以运动加速度的方差来推导，此时只要估计一个加速度的方差即可；
R矩阵是传感器测量的不准确度，每个每个传感器都会给出测量对向的准确度，直接转换就能用。

附上某bilibili课件图片





CRTV等汽车运动模型

这里再扩展说一下模型的作用，模型决定了预测的计算公式。只要是模型就一定有误差，但ν～N(0,Q)可能不成立，那么就要找到平均值完成运算，并在末端加上ν～N(0,Q)。总之要明确把误差零均值化！！！（算标准差也是先零均值化的）然后基于ν～N(0,Q)完成P’预测。

  • 对原始数据进行中心化（Zero-centered(零均值化)或者Mean-subtraction（subtraction表示减去））处理,中心化（零均值化）后的数据均值为零
  • 对原始数据进行标准化（Standardization或Normalization(归一化)）处理
  • https://blog.csdn.net/GoodShot/article/details/80373372

  • 图源：https://blog.csdn.net/qq_42258099/article/details/99601467

先说明Q，再说明一下FPFTFPF^TFPFT。

前面说明了P是用来说明X的数值精确度以及参量之间的相关性。P‘(P^)P‘(\hat{P})P‘(P^)就是要更新P并考虑上预测误差。

已知ν～N(0,Q)，v即为误差，那么根据协方差公式，求出Q，描述X^\hat{X}X^的协方差矩阵，其中对角代表各参数值的精确度，越小越精确；对角外其他数值代表两个参数间的相关性，0代表不相关，数值越大相关性越强。

FPFTFPF^TFPFT,通过对比说明:

假设p～N(u,σ)，X^\hat{X}X^就相当于这里的u均值，σ相当于P^\hat{P}P^方差（协方差），表示p是关于X的方差为σ的分布

我们可以对于p进行拉伸、缩放、平移等操作，那么u，σ都会随之而变换，其中σ不会因平移而变化，但对于拉伸、缩放就会发生变化，而FPFTFPF^TFPFT就是这样的作用，F是状态变化.

例子

% 初始化参数
delta_t=0.1;   %采样时间
t=0:delta_t:5;
N = length(t); % 序列的长度
sz = [2,N];    % 信号需开辟的内存空间大小  2行*N列  2:为状态向量的维数n
g=10;          %加速度值
x=1/2*g*t.^2;      %实际真实位置
z = x + sqrt(10).*randn(1,N); % 测量时加入测量白噪声Q =[0 0;0 9e-1]; %假设建立的模型  噪声方差叠加在速度上 大小为n*n方阵 n=状态向量的维数
R = 10;    % 位置测量方差估计，可以改变它来看不同效果  m*m      m=z(i)的维数A=[1 delta_t;0 1];  % n*n
B=[1/2*delta_t^2;delta_t];
H=[1,0];            % m*nn=size(Q);  %n为一个1*2的向量  Q为方阵
m=size(R);% 分配空间
xhat=zeros(sz);       % x的后验估计
P=zeros(n);           % 后验方差估计  n*n
xhatminus=zeros(sz);  % x的先验估计
Pminus=zeros(n);      % n*n
K=zeros(n(1),m(1));   % Kalman增益  n*m
I=eye(n);% 估计的初始值都为默认的0，即P=[0 0;0 0],xhat=0
for k = 9:N           %假设车子已经运动9个delta_T了，我们才开始估计% 时间更新过程xhatminus(:,k) = A*xhat(:,k-1)+B*g;Pminus= A*P*A'+Q;% 测量更新过程K = Pminus*H'*inv( H*Pminus*H'+R );xhat(:,k) = xhatminus(:,k)+K*(z(k)-H*xhatminus(:,k));P = (I-K*H)*Pminus;
end

多元高斯分布协方差矩阵

两个随机变量的协方差表现的是两个变量之间的相关关系；自己跟自己的协方差就是方差。协方差可以求解相关系数。

独立关系：两个随机变量不存在任何关系（包括线性关系），独立必不相关
相关关系：存在线性关系。不相干未必独立
二维随机变量，4个二阶中心矩组成了协方差矩阵

多元高斯分布，高斯公式中的方差也变成了协方差，对应上面三张图的协方差矩阵分别如下：

拓展卡尔曼

关于泰勒展开的故事：

  • https://www.zhihu.com/question/25627482

    泰勒展开式就是把一个三角函数或者指数函数或者其他比较难缠的函数用多项式替换掉。

有一个原函数 f(x)f(x)f(x)，我再造一个图像与原函数图像相似的多项式函数 g(x)g(x)g(x) ,为了保证相似，我只需要保证这俩函数在某一点x0x_0x0​的初始值相等，1阶导数相等，2阶导数相等，……n阶导数相等。

麦克劳林级数 是 x0=0x_0=0x0​=0 的泰勒级数。我们上面的例子都是 麦克劳林级数。

泰勒展开，初值对估计的影响

  • https://www.matongxue.com/madocs/206.html

    直接上结论：

推导过程（可以忽略）

1、收敛

2、奇点

3、 奇点与收敛圆
通过奇点来判断泰勒级数的收敛，非常漂亮的数学结论，由柯西证明的泰勒级数的收敛半径：

言归正传：

卡尔曼滤波的优点有很多，但同样也有一个问题，只适用于线性模型，对上面几种模型来说，出现非线性项（比如cos, sin）时，就无法利用上面的结论了。处理非线性问题最常见的一种思路就是线性化，而对线性化我们有一种强大的工具-泰勒展开。

一元函数：

二元函数：

多元函数：

把Taylor展开式写成矩阵的形式：

EKF的线性化采用一阶泰勒展开

KF与EKF的区别如下：

预测：x^=Fx+u\hat{x}=Fx+ux^=Fx+u用x^=f(x,u)\hat{x}=f(x,u)x^=f(x,u)代替；其余状态转移矩阵FFF用雅克比矩阵FjF_jFj​代替。
修正：将状态映射到测量的Hx^H\hat{x}Hx^用h(x^)h(\hat{x})h(x^)代替；其余观测矩阵HHH用雅克比矩阵HjH_jHj​。

假设状态有n维，则求法如下：


其中，
非线性函数f(x,u)，h(x^)f(x,u)，h(\hat{x})f(x,u)，h(x^)用非线性得到了更精准的状态预测值、映射后的测量值；
线性变换Fj，HjF_j，H_jFj​，Hj​线性变换使得变换后的x，zx，zx，z仍满足高斯分布的假设。

实际上python中有专门计算梯度和雅克比矩阵的库numdifftools

#! /usr/bin/env python
# -*- coding: UTF-8 -*-
import numpy as np
import math
import numdifftools as nddt = 0.5
def f(x):return np.array([x[0]+math.cos(x[2])*x[3]*dt,x[1]+math.sin(x[2])*x[3]*dt,x[2], x[3]])
Jfun = nd.Jacobian(f)
print(Jfun([1,2,math.pi,3]))

过程总结


可以看出，EKF依然是在KF的框架内进行的改进，所以思路与KF是完全一致的。只是这里写HjH_jHj​描述与这里写FjF_jFj​描述的计算方法不同。

EKF-近似最优卡尔曼增益

此处为什么是近似最优卡尔曼增益而不是最优卡尔曼增益。这是因为计算HjH_jHj​和FjF_jFj​的时候理论上应该计算函数HjH_jHj​和FjF_jFj​的雅可比矩阵。但是实际操作起来非常困难，特别是对于一些复杂的非线性系统。因此往往采用泰勒展开去一阶线性的部分。由于近似，得到的卡尔曼在增益也就不是最优卡尔曼增益，而是近似最优卡尔曼增益。这就直接导致了EKF在高度非线性系统下性能锐减的必然结果。而且系统初始状态估计错误或者说建模不正确，EKF也会迅速发散。

状态向量的选取经验（可以略过）



EKF 例子

假设现在我们有一辆车做匀速直线行驶，则过程模型为：

对应的雅克比矩阵为：

由于是匀速过程，那么这里uk=0u_k=0uk​=0对于过程噪声，实际上就是车子加速度aaa，因此可以认为协方差矩阵Q满足Q=E(vvT)Q=E(vv^T)Q=E(vvT) ，其中

对于观测矩阵，一般汽车就只能读到实时速度，因此H=[0,0,0,1]H=[0,0,0,1]H=[0,0,0,1] ，对应的协方差矩阵为R=δv2R=δ^2_vR=δv2​ ，R为观测噪声

在测量值只有速度，存在过程噪声和测量噪声的情况下，利用卡尔曼滤波去得到一个最优的状态估计

#! /usr/bin/env python
# -*- coding: UTF-8 -*-
import numpy as np
import matplotlib.pyplot as plt
import math## x,p,dt,theta,v参数
x = np.matrix([[0.0, 0.0, 0.0, 0.0]]).T #状态变量[x, y, theta, v]
P = np.diag([1.0, 1.0, 1.0, 1.0]) #误差矩阵
dt = 0.1 # 时间间隔
theta = math.pi/6
v = 10 # 速度,匀速运动
F = np.matrix([[1.0, 0.0, -v * math.sin(theta) * dt, math.cos(theta) * dt],[0.0, 1.0, v * math.cos(theta) * dt,  math.sin(theta) * dt],[0.0, 0.0, 1.0, 0.0],[0.0, 0.0, 0.0, 1.0]]) #状态转移矩阵H = np.matrix([[0.0, 0.0, 0.0, 1.0]]) #观测矩阵
R = 0.25 # 观测方差
deltav = 0.5
v = np.matrix([[0.5*dt**2],[0.5*dt**2],[0],[dt]])
Q = v*v.T*deltav**2 #过程协方差矩阵
I = np.eye(4)
m = 200 # 测量数据数## 数据存储变量
xt = []
yt = []
thetat= []
vt= [] #v估计值
Zv = [] #v观测值## 保存过程变量
def saveStates(x, Z):xt.append(float(x[0]))yt.append(float(x[1]))thetat.append(float(x[2]))vt.append(float(x[3]))Zv.append(float(Z[0]))## 滤波
def KFfilter():global x, P, S, K, Z, yfor n in range(m):## 预测过程x = F*xP = F*P*F.T + QS = H*P*H.T + RK = (P*H.T) * np.linalg.pinv(S)# 更新过程Z = v + np.random.randn(1)  # 测量值y = Z - (H*x)x = x + (K*y)P = (I - (K*H))*PsaveStates(x, Z)def plot_x():fig = plt.figure(figsize=(16,9))plt.plot(range(m),vt, label='$estimateV$')plt.plot(range(m),Zv, label='$measurementV$')plt.axhline(v, color = 'red', label='$trueV$')plt.xlabel('Filter Step')plt.title('KF Filter')plt.legend(loc='best')plt.ylim([5, 15])plt.ylabel('Velocity')plt.show()if __name__ =='__main__':KFfilter()plot_x()

另一种EKF_error state KF

error state KF 非线性卡尔曼是对状态误差的转移。

多传感器融合-lidar+Radar

  • 链接：https://blog.csdn.net/Young_Gy/article/details/78468153
    本文将以汽车跟踪为例，目标是知道汽车时刻的状态x=(px,py,vx,vy)x=(p_x,p_y,v_x,v_y)x=(px​,py​,vx​,vy​)。已知的传感器有lidar、radar。

lidar：笛卡尔坐标系。可检测到位置，没有速度信息。其测量值z=(px,py)z=(p_x,p_y)z=(px​,py​)。
radar：极坐标系。可检测到距离，角度，速度信息，但是精度较低。其测量值z=(ρ,ϕ,ρ˙)z=(ρ,ϕ,ρ˙)z=(ρ,ϕ,ρ˙)，图示如下。

初始化

初始化，指在收到第一个测量值后，对状态xxx进行初始化。初始化如下，同时加上对时间的更新。
对于radar来说，

对于lidar来说，

预测

预测主要涉及的公式是：

x^=Fx\hat{x}=Fxx^=Fx
Pk=FPk−1FT+Q{P_k}=FP_{k-1}F^T+QPk​=FPk−1​FT+Q

需要求解的有三个变量：F、P、Q。

F表明了系统的状态如何改变，这里仅考虑线性系统，F易得

P表明了系统状态的不确定性程度，用x的协方差表示，这里自己指定为

Q表明了x^=Fx\hat{x}=Fxx^=Fx未能刻画的其他外界干扰。本例子使用线性模型，因此加速度变成了干扰项。x^=Fx\hat{x}=Fxx^=Fx中未衡量的额外项目v为：

v 服从高斯分布N(0,Q)。

修正

lidar使用了KF

修正当下这里牵涉到的公式主要是：
y~=z−Hx^\tilde{y}=z−H\hat{x}y~​=z−Hx^
S=HPk−1HT+RS=HP_{k-1}H^T+RS=HPk−1​HT+R
K=Pk−1HTS−1K=P_{k-1}H^TS^{−1}K=Pk−1​HTS−1
x^=x^+Ky~\hat{x}=\hat{x}+K\tilde{y}x^=x^+Ky~​
Pk=(I−KH)Pk−1P_{k}=(I−KH)P_{k-1}Pk​=(I−KH)Pk−1​

需要求解的有两个变量：H、R。

H 表示了状态空间到测量空间的映射。

R表示了测量值的不确定度，一般由传感器的厂家提供，这里lidar参考如下：

radar使用了EKF

修正当下这里牵涉到的公式主要是：
y~=z−f(x^)\tilde{y}=z−f(\hat{x})y~​=z−f(x^)
S=HjPHjT+RS=H_jPH^T_j+RS=Hj​PHjT​+R
K=PHjTS−1K=PH^T_jS^{−1}K=PHjT​S−1
x^=x^+Ky~\hat{x}=\hat{x}+K\tilde{y}x^=x^+Ky~​
Pk=(I−KHj)Pk−1P_k=(I−KH_j)P_{k-1}Pk​=(I−KHj​)Pk−1​

区别与上面lidar的主要有：

  • 状态空间到测量空间的非线性映射f(x)
  • 非线性映射线性化后的Jacob矩阵
  • radar的RradarR_radarRr​adar

状态空间到测量空间的非线性映射f(x)如下:

非线性映射线性化后的Jacob矩阵HjH_jHj​

R 表示了测量值的不确定度，一般由传感器的厂家提供，这里radar参考如下：

传感器融合实例

多传感器融合的示例如下，需要注意的有：

  • lidar和radar的预测部分是完全相同的
  • lidar和radar的参数更新部分是不同的，不同的原因是不同传感器收到的测量值是不同的
  • 当收到lidar或radar的测量值，依次执行预测、更新步骤
  • 当同时收到lidar和radar的测量值，依次执行预测、更新1、更新2步骤

3月15日 卡尔曼与多元传感器融合相关推荐

  1. 飞控学习常见典型问题集QA——无名创新（2022年4月15日）

    飞控学习常见典型问题集Q&A--无名创新(2022年4月15日) 1.第一次启动FS I6遥控器,进入不了界面一直嘟嘟,请问这是什么情况呢? 先把上面的英文翻译一下,警告:请把所有的开关切换到 ...

  2. 互联网晚报 | 9月15日 星期三 | iPhone 13正式发布；天猫双11正式启动商家报名；华为推出鸿蒙矿山操作系统“矿鸿”...

    今日看点 ✦ 天猫双11发布10项商家扶持举措,9月15日正式启动商家报名 ✦ 华为宣布推出鸿蒙矿山操作系统"矿鸿",HarmonyOS正式走向B端 ✦ 超4万平米.20万件商品, ...

  3. 武汉大学无线传感实验床项目招标公告(开标时间2015年1月15日)

    根据国家采购与招投标法律法规的有关规定,武汉大学拟对计算机学院无线传感实验床项目进行公开招标,欢迎具备相应资质和实力的供应商参加投标.现将有关事项公告如下: 一.招标范围: (主要技术指标:见附件) ...

  4. 2月15日Python线上峰会免费学！6场精华分享，用代码“抗”疫

    截至截止2月12号09时43分,新型冠状病毒在全国已确诊44726例,疑似病例已达21675例.而专家所说的"拐点"始终未至,受疫的影响,各大公司开启远程办公模式,将返回工作场所办 ...

  5. 第八届“数学、计算机与生命科学交叉研究” 青年学者云论坛(5月15日9:00-16:40)...

    第八届"数学.计算机与生命科学交叉研究" 青年学者论坛--特别云论坛 一. 会议简介 "数学.计算机与生命科学交叉研究"青年学者论坛旨在加强从事 "数 ...

  6. 魔兽世界10月15日服务器维护,《魔兽世界》怀旧服将于10月15日开放新服务器

    近日<魔兽世界>官网发布公告,宣布将于10月15日开放新的怀旧服服务器,并对即将到来的厄运之槌副本进行预告. 厄运之槌预告 盘曲纵深,分为三个大区的经典怀旧服五人地下城--厄运之槌将于10 ...

  7. 2021年3月15日 腾讯PCG运营开发实习面试（一、二、三面）（面经含总结）

    title: 2021年3月15日 腾讯PCG运营开发实习面试(一.二.三面) tags: 面经 2021年3月15日 腾讯PCG运营开发实习面试(一面2h) 自我介绍你能讲一下Java中的集合,你都 ...

  8. 分享Silverlight/WPF/Windows Phone一周学习导读(10月1日-10月15日)

    分享Silverlight/WPF/Windows Phone一周学习导读(10月1日-10月15日) 本周Silverlight学习资源更新: [Silverlight入门系列]ListboxIte ...

  9. 分享Silverlight/WPF/Windows Phone一周学习导读(8月15日-8月19日)

    分享Silverlight/WPF/Windows Phone一周学习导读(8月15日-8月19日) 本周Silverlight学习资源更新: Silverlight Tools 4安装时的错误提示 ...

  10. 11月15日BCH协议升级提案又双叒啜来了，请查收！

    ​​BCH告别了5月的纷扰,迎来了新的篇章.对于BCH来说,一年中有两件大事,一是5月15日的升级,二是11月15日的升级.上半年的网络升级已经顺利完成并告一段落,下半年的升级工作正在被BCH开发者写 ...

最新文章

  1. 图解VC++6.0和ACE 5.4 开发入门
  2. 通讯录javafx和mysql_用javafx实现通讯录
  3. 判断iframe是否加载完毕的方法（兼容ie和Firefox）
  4. C#秘密武器之反射——基础篇
  5. python提交事务_事务提交和回滚
  6. 【机器学习技术】高斯过程初探
  7. 多个android客户端使用的数据库,android – 将Firebase数据库与本地数据库一起使用...
  8. cartographer安装_【ROS-SLAM】Cartographer ROS官方文档翻译学习（１）——下载与安装...
  9. mysql优化--避免数据类型的隐式转换
  10. MySQL的用户密码过期password_expired功能
  11. 模拟退火算法+大规模邻域算法求解大规模固定节点的路径规划问题matlab代码
  12. 树莓使用CUPS服务搭建打印机服务器
  13. 2018最新Web前端经典面试试题及答案-史上最全前端面试题(含答案)--转载
  14. 索骥馆－OFFICE系列之《北风网Excel高端应用培训:多条件约束报表自动统计系统分析与制作》共4章更新完
  15. 解决Program received signal SIGSEGV, Segmentation fault.的辛酸
  16. ★ZOJ 3380 Patchouli's Spell Cards 详细题解 （递推+组合数求方案数）
  17. Python-Django毕业设计航空订票系统（程序+Lw)
  18. RuntimeError: cublas runtime error : the GPU program failed to execute at /tmp/pip-req-build-jh50bw2
  19. 阿里巴巴矢量图标（iconfont）添加后显示不出来的解决办法
  20. echart折线图 -- 每条线分别对应不同x、y轴数据

热门文章

  1. Linux 系统常用命令
  2. nit计算机考试题目,NIT计算机考试大纲
  3. 阿里开源 10 年，这封信里也有 Flink 的身影。
  4. Kotlin实战【一】Kotlin定义与目的
  5. 终于，我们这代程序员在上海各奔东西
  6. 层次聚类 簇数_聚类（一）：K-means、层次、DBSCAN、均值漂移、K-Means 与 KNN
  7. ant接口用什么天线_拆解测量真假钻石手台天线SRH805S
  8. linux eof打印列表,Linux：结合cat和EOF输出到文本文件
  9. c++开发软件_Windows下学习C语言有哪些集成开发软件？
  10. java 计算两个日期之间的月份_Java 8计算两个日期之间的月份