锐单电子商城 , 一站式电子元器件采购平台!
  • 电话:400-990-0325

手撕卡尔曼滤波器

时间:2022-11-21 17:00:00 9qb型传感器hpk无功功率变送器

手撕卡尔曼滤波器

卡尔曼滤波器(Kalman Filter),字面上看,Filter滤波器一词不能很好地体现其特点。用一句话来说,卡尔曼滤波器Optimal Recursive Data-Processing Algorithm,即最优化 递归 数字处理 算法像是一种观测器,而不是一般意义上的滤波器。特别是在导航中,卡尔曼滤波器被广泛使用。它的广泛应用是因为世界中存在大量的不确定性,当我们描述一个系统时,这个不确定性主要体现在三个方面:

  1. 没有完美的数学模型
  2. 系统的扰动无法控制,难以建模
  3. 测量传感器存在误差

递归算法

下面是一个例子,用同一把尺子多次测量同一枚硬币的直径 z k z_k zk 表示第k次测量结果。由于各种误差,测量得到:
z 1 = 50.1 m m z 2 = 50.4 m m z 3 = 50.2 m m \begin{aligned} z_1=50.1mm \\ z_2=50.4mm \\ z_3=50.2mm \end{aligned} z1=50.1mmz2=50.4mmz3 =50.2mm
此时如果要估计真实结果,自然而然地会想到取平均值。用 x ^ k \hat{x}_k x^k 表示第k次的估计值,可以得到:
x ^ k = 1 k ( z 1 + z 2 + ⋯ + z k ) = 1 k ( z 1 + z 2 + ⋯ + z k − 1 ) + 1 k ( z k ) = k − 1 k 1 k − 1 ( z 1 + z 2 + ⋯ + z k − 1 ) + 1 k ( z k ) = k − 1 k x ^ k − 1 + 1 k ( z k ) = x ^ k − 1 + 1 k ( z k − x ^ k − 1 ) \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{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)\\ &= \hat{x}_{k-1} + \frac{1}{k}(z_k-\hat{x}_{k-1}) \end{aligned} x^k=k1(z1+z2++zk)=k1(z1+z2++zk1)+k1(zk)=kk1k11(z1+z2++zk1)+k1(zk)=kk1x^k1+k1(zk)=x^k1+k1(zkx^k1)
上式中,第三行 1 k − 1 ( z 1 + z 2 + ⋯ + z k − 1 ) \frac{1}{k-1}(z_1+z_2+\cdots+z_{k-1}) k11(z1+z2++zk1) 就是 k − 1 k-1 k1 次的平均值 x ^ k − 1 \hat{x}_{k-1} x^k1

观察最后一行结论, k ↑ , 1 k − 1 → 0 , x ^ k → x ^ k − 1 k\uparrow, \frac{1}{k-1}\to0,\hat{x}_k\to\hat{x}_{k-1} k,k110,x^kx^k1 ,也就是说,随着k的增加,此时拥有了大量的数据,对估计的结果就比较有信心了,测量的结果就不是很重要了。相反,如果k比较小, 1 k − 1 \frac{1}{k-1} k11 就会比较大,测量结果 z k z_k zk 就会起到很大的作用,尤其是测量结果和估计值差距比较大的时候。

K k = 1 k − 1 K_k=\frac{1}{k-1} Kk=k11 ,则此时公式可以表示为:
x ^ k = x ^ k − 1 + K k ( z k − x ^ k − 1 ) \hat{x}_k = \hat{x}_{k-1} + K_k(z_k-\hat{x}_{k-1}) x^k=x^k1+Kk(zkx^k1)
上式表示的含义为:当前的估计值 = 上一次的估计值 + 系数 * ( 当前测量值 - 上一次的估计值 ) ,其中的 K k K_k Kk 就是卡尔曼增益/因数(Kalman Gain),通过这个公式可以看出,新的估计值 x ^ k \hat{x}_k x^k 与上一次的估计值 x ^ k − 1 \hat{x}_{k-1} x^k1 有关,上一次的又与上上次的有关,这就是一种递归思想(Recursive),这也是卡尔曼滤波器的优势,他不需要追溯很久以前的数据,只需要上一次的就可以。下面来讨论一下这个 K k K_k Kk

引入两个误差:

  1. 估计误差 e E S T e_{EST} eEST (e代表误差error,EST代表估计estimate)
  2. 测量误差 e M E A e_{MEA} eMEA (e代表误差error,MEA代表测量measurement)

K k K_k Kk 可以表示为
K k = e E S T k − 1 e E S T k − 1 + e M E A k K_k = \frac{e_{EST_{k-1}}}{e_{EST_{k-1}}+e_{MEA_{k}}} 元器件数据手册
IC替代型号,打造电子元器件IC百科大全!

相关文章