话不多说,首先推荐两个博客:
https://blog.csdn.net/u012554092/article/details/78290223
https://blog.csdn.net/phker/article/details/48468591
以上两个公式都对公式进行了一定的说明,但是说真的,公式的说明还是太简洁了,很多地方看得还是很模糊,但是还好有例子,所以定性的认识还是很容易的。
首先给一个图(是从上面两个博客中截的图)
什么是卡尔曼滤波呢?实际上就是我们在知道k-1时刻值的基础上,估计k时刻的最优值。
给定初始条件:1.k-1时刻的最优值
x
k
−
1
x_{k-1}
xk−1,其误差为
a
1
a_1
a1(最优值是说我们能得到的误差最小的值,并不是说是完全正确的值,这一点一定要注意),这对应于上图中的k-1时刻的温度
2
3
o
23^o
23o,然后我们根据我们的经验公式预测下一个时刻k的值为
x
k
x_k
xk,然后我们的预测误差为
a
2
a_2
a2(我们预测的值肯定是会有误差的)这对应上图中k时刻的预测温度
2
3
o
23^o
23o预测误差
4
o
4^o
4o,然后我们还通过仪器获取了一个测量值
x
^
k
\hat{x}_{k}
x^k,当然仪器也是会有误差的即
a
3
a_3
a3这对应上图中的
2
5
o
25^o
25o和
4
o
4^o
4o.
然后我们要通过上述六个数据求得k时刻的最优值这就是卡尔曼滤波的目的。
接着我们求出我们预测数据的绝对误差,我们的k-1时刻温度的误差为3,而我们基于k-1时刻预测的温度具有的4的误差,那么误差再叠加一次误差,k时刻的绝对误差为 a 4 = 3 2 + 4 2 = 5 a_4=\sqrt{3^2+4^2}=5 a4=32+42=5,然后我们k时刻的测试误差为4,我们现在将六个数据处理成4个数据了,然后我们要根据这四个数据求出最优的估计。
然后我们根据这两个数据误差的大小权重来选择选取最优的数据,我们选择的标准并不是简单的线型插值方法,而是非线性插值。
首先计算插值比例:
k
g
=
a
4
2
a
4
2
+
a
3
2
k_g=\frac{a_4^2}{a_4^2+a_3^2}
kg=a42+a32a42
k
g
=
5
2
/
(
5
2
+
4
2
)
=
0.78
k_g=5^2/(5^2+4^2)=0.78
kg=52/(52+42)=0.78
即误差平方的比例。
然后再根据比例进行线型插值:
x
k
k
=
x
k
+
k
g
∗
(
x
k
^
−
x
k
)
x_{kk}=x_k+k_g*(\hat{x_k}-x_k)
xkk=xk+kg∗(xk^−xk)
23
+
0.78
∗
(
25
−
23
)
=
24.56
23+0.78*(25-23)=24.56
23+0.78∗(25−23)=24.56
这样我们就计算出最优值了,但是为了计算出k+1时刻的误差值,我们好需要计算出该最优值的误差,计算公式为:
(
1
−
k
g
)
∗
5
2
\sqrt{(1-k_g)*5^2}
(1−kg)∗52
(
1
−
0.78
)
∗
5
2
=
2.35
\sqrt{(1-0.78)*5^2}=2.35
(1−0.78)∗52=2.35
这样我们就获得了,下一次迭代的所需数据了。
2018.10.2更新:
不小心看到了对卡尔曼滤波更好的解释博客,所以想再写写,话不多说将该博客的链接放在这里,供大家参考(毕竟那个博客不好百度出来):
https://blog.csdn.net/u010720661/article/details/63253509
以下类容来自于上上面这篇博客的理解:
1.假设我们现在有一个k-1时刻的最优数据,但是该数据是不精确的,我们用高斯函数来描述该数据的不确定性,即以该数据当前值为均值,该数据的协方差矩阵为高斯函数的协方差矩阵(记住,多维高斯函数的方差为协方差矩阵)如下图所示:
2.然后我们根据经验对k时刻的数据进行预测:
这里
F
k
F_k
Fk是经验矩阵,在上述博客里面是速度和时间相关的矩阵,而协方差矩阵也按照下面规则变了。
3.以上是按照经验进行估计的k时刻可能的数据,但是在这期间可能会出现我们经验之外的变量,在上述博客里面这个变量时加速度a,然后我们将其考虑进来,k时刻的预测变量就变成了如下式子:
注意了,这时协方差矩阵是不变的,因为,变量加上一个值,该变量的方差是不会变的啦。
4.但是我们还要考虑其他一些意外因素比如天灾等我们无法事先预测的事情,在上述博文里面的是风。但是这些意外因素不影响我们的均值,它只会影响我们的协方差,即将我们的误差范围变大。
5.然后我们的传感器在k时刻会有一个数据,我们能够将传感器的数据当成我们的最终结果吗?当然是不能啦,因为传感器也会有误差,搞不好还会故障出错,所以我们将中和预测值和传感器数据得出一个最优值。
如上所说,传感器也有误差,我们也用高斯函数来描述该传感器传感器的数据用
z
k
z_k
zk和
R
k
R_k
Rk来描述。
如果预测数据和传感器数据都是可信的,那么他们必定有很大的交集,我们现在要做的就是融合这两个高斯函数,我们采取的融合方法是直接相乘:
注:上述公式和图片全部来自上述博客(侵删)。
注:在进行融合之前我们要将预测的数据进行单位的转化,将预测数据和传感器数据划到同一个单位尺度在进行融合,在上述博客中是利用
H
k
H_k
Hk矩阵进行处理的。
以上就是卡尔曼滤波的思想,卡尔曼滤波的公式,请阅读上述博客吧,上面那个博客公式讲的非常详细。