自己建设网站引流,安卓html编辑器中文版,软文营销定义,wordpress模板在线编辑后端之卡尔曼滤波
前言
在很久之前#xff0c;人们刚结束信息传递只能靠信件的时代#xff0c;通信技术蓬勃发展#xff0c;无线通信和有线通信走进家家户户#xff0c;而著名的贝尔实验室就在这个过程做了很多影响深远的研究。为了满足不同电路和系统对信号的需求#…后端之卡尔曼滤波
前言
在很久之前人们刚结束信息传递只能靠信件的时代通信技术蓬勃发展无线通信和有线通信走进家家户户而著名的贝尔实验室就在这个过程做了很多影响深远的研究。为了满足不同电路和系统对信号的需求比如去除噪声或者区分不同频率的信号滤波器就诞生了而贝尔实验室就是这一领域研究的先行者。早期的滤波器是电子滤波器是由电阻电感电容等电子元件组成的物理电路其电路图大概长这样 这个是由一个电容和一个电阻组成的RC滤波器。而装在实际家电或者设备中的滤波器大概如下图这个是一个包含高通和低通滤波器的信号分离装置。 随着技术的发展主要是计算机的兴起相比于处理原始模拟信号人们更愿意处理数字信号这可以带来更高的处理速度更低的成本和更高的精度于是数字滤波器诞生了。数字滤波器是对数字信号进行滤波处理以得到期望的响应特性的离散时间系统。这时候数字模拟器还是依靠基本的一些电路元件比如寄存器延时器加法器等但其工作的领域已变为经过数模转换器转化后的数字信号域了后面广泛用于收音机蜂窝电话等设备中。
数字滤波器早期主要处理信号而信号都是一些波形这也是滤波的由来。后面随着人们对滤波器的扩展出现了另外一种形式的数字滤波器其工作过程包含状态空间模型称为状态空间滤波器状态空间滤波器的一个典型例子是Rudolf Kalman在1960年提出的卡尔曼滤波器。这时候虽然没有了波主要是空间状态但按传统这个名称还是保留了下来。 最后随着计算机的发展中央处理器CPU集成了各种计算单元所有计算任务都可以交给它数字滤波器就不用单独保留如寄存器加法器等元件了在保留了算法原理和流程之后羽化成仙成为了滤波算法。
卡尔曼滤波算法推导简明版
发明了卡尔曼滤波算法的人叫做鲁道夫·埃米尔·卡尔曼是一个匈牙利人。 卡尔曼滤波是基于马尔科夫假设的即下一时刻状态只与上一时刻有关。其主要针对于线性高斯系统计算的流程如下 假设状态 X \mathbf{X} X的转移方程为 其中 F k \mathbf{F}_{k} Fk为状态转移矩阵而 P k \mathbf{P}_{k} Pk为 X k \mathbf{X}_{k} Xk的方差 X \mathbf{X} X只表示自身的状态另外一些系统还会有外部控制因数比如火车减速时速度是状态量但可以有刹车装置进行减速如果系统存在这部分控制因数需要把这部分加到状态转移中 x ^ k F k x ^ k − 1 B u k P k F k P k − 1 F k T \begin{aligned} \hat{\mathbf{x}}_{k} \mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} Bu_k\\ \mathbf{P}_{k} \mathbf{F}_{\mathbf{k}} \mathbf{P}_{k-1} \mathbf{F}_{k}^{T} \end{aligned} x^kPkFkx^k−1BukFkPk−1FkT
另外 X \mathbf{X} X中包含环境的一些未知变量我们假设为噪声同时噪声分布假设服从高斯分布于是有如下方程 x ^ k F k x ^ k − 1 B u k w k P k F k P k − 1 F k T R \begin{aligned} \hat{\mathbf{x}}_{k} \mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} Bu_k w_k\\ \mathbf{P}_{k} \mathbf{F}_{\mathbf{k}} \mathbf{P}_{k-1} \mathbf{F}_{k}^{T} R \end{aligned} x^kPkFkx^k−1BukwkFkPk−1FkTR
卡尔曼滤波中还需要使用观测方程来更新一般观测方程是需要从状态变为观测量的即需要有一个观测量到状态的转换但很多时候这个转换方程都没有但这并不影响我们假设如果没有转换方程到时候直接把转换矩阵设为单位矩阵就行那现在假设观测方程为 z k H ∗ x k v k z_k H*x_kv_k zkH∗xkvk
其中 H H H为观测方程 v k v_k vk为观测的噪声分布假设其服从 v k ∼ N ( 0 , Q ) v_k\sim N\left(0,Q\right) vk∼N(0,Q)即零均值方差为Q的高斯分布
到这里需要说明一点上一次滤波的结果会作为下一次滤波的初始值即由上一次后验概率通过状态转移矩阵与控制向量变为目前的先验值所以原来的观测方程需要变为 x ^ k ˉ F k x ^ k − 1 B u k w k P k ˉ F k P k − 1 F k T R \begin{aligned} \hat{\mathbf{x}}_{\bar{k}} \mathbf{F}_{k} \hat{\mathbf{x}}_{k-1} Bu_k w_k\\ \mathbf{P}_{\bar{k}} \mathbf{F}_{\mathbf{k}} \mathbf{P}_{k-1} \mathbf{F}_{k}^{T} R \end{aligned} x^kˉPkˉFkx^k−1BukwkFkPk−1FkTR
其中下标不带横杠的表示后验值带横杠的代表先验值这样就由上一时刻的最优估计得到了当前预测的先验状态及先验方差。 对于观测方程其方差主要由观测方差决定即 P z k R P_{z_k} R PzkR
zk的均值为Hxk,即zk服从N(Hxk,R)的分布。
这两个分布都是高斯分布一个是状态的先验分布一个是传感器测量的分布这个测量与xk的状态也有关现在要求这两个分布的联合分布并求其最大值很简单把两个分布乘起来由于高斯分布的乘积还是高斯分布取其均值处就是概率最大的状态。 乘积的部分就是所说的状态更新首先由高斯分布的公式可得 N ( x , μ , σ ) 1 σ 2 π e − ( x − μ ) 2 2 σ 2 \mathcal{N}(x, \mu, \sigma)\frac{1}{\sigma \sqrt{2 \pi}} e^{-\frac{(x-\mu)^{2}}{2 \sigma^{2}}} N(x,μ,σ)σ2π 1e−2σ2(x−μ)2 那么两个分布相乘系数结果为 其中均值u就是概率最大时的值σ就是方差其中这个式子就是最后我们要求的但这个式子有点复杂于是用一个系数化简为 其中k称为卡尔曼增益u0为预测值u1为观测。可以看到k为0到1之间的数分子为预测的方差如果预测方差越大则越向观测值靠拢如果预测方差越小则越向预测值靠拢。
截至目前我们有用矩阵 ( μ 0 , Σ 0 ) ( H k x ^ k , H k P k H k T ) \left(\mu_0, \Sigma_0\right)\left(H_k \hat{x}_k, H_k P_k H_k^T\right) (μ0,Σ0)(Hkx^k,HkPkHkT)预测的分布有用传感器读数 ( μ 1 , Σ 1 ) ( z ⃗ k , R k ) \left(\mu_1, \Sigma_1\right)\left(\vec{z}_k, R_k\right) (μ1,Σ1)(z k,Rk)预测的分布。把它们代入上节的矩阵等式中
相应的卡尔曼增益就是
两个式子左边都有不少Hk矩阵同时把这个矩阵去掉则K变为 于是我们得到最后卡尔曼更新的公式 K k P k − H T H P k − H T R x ^ k x ^ k ˉ K k ( z k − H x ^ k ˉ ) P k ( I − K k H ) P k ˉ \begin{array}{c} K_{k}\frac{P_{k}^{-} H^{T}}{H P_{k}^{-} H^{T}R} \\ \hat{x}_{k}\hat{x}_{\bar{k}}K_{k}\left(z_{k}-H \hat{x}_{\bar{k}}\right) \\ P_{k}\left(I-K_{k} H\right) P_{\bar{k}} \end{array} KkHPk−HTRPk−HTx^kx^kˉKk(zk−Hx^kˉ)Pk(I−KkH)Pkˉ
其中计算K的都是使用先验方差R为传感器方差。 Zk为实际观测值Hxk为预测的观测值。 最后使用K及先验方差得到后验方差及后验均值。
参考链接
https://zhuanlan.zhihu.com/p/39912633 https://www.guyuehome.com/
