00 前言
在RoboMaster比赛中,由于工业相机帧率、算法效率、信息传输速率等影响,我们通常需要对目标进行预测,才能更好的击中装甲板。
首先感谢robomaster比赛中各个学校开源的代码和各类教程和代码,共同进步,以及感谢卡尔曼滤波作者Rudolf Emil Kalman。
原论文地址:http://www.cs.unc.edu/~welch/kalman/media/pdf/Kalman1960.pdf
如本文出现错误欢迎指出,一起学习交流共同进步
01 卡尔曼滤波简介
百度百科:卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响,所以最优估计也可看作是滤波过程。
数据滤波是去除噪声还原真实数据的一种数据处理技术,Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中,估计动态系统的状态。由于它便于计算机编程实现,并能够对现场采集的数据进行实时的更新和处理,Kalman滤波是目前应用最为广泛的滤波方法,在通信,导航,制导与控制等多领域得到了较好的应用。
卡尔曼滤波的应用领域非常广泛,在航空航天、信息技术等领域尤为广泛。
02 卡尔曼滤波概要
由于我们在实际工程中,测量往往存在很多不确定性,这些不确定性包括有1)不存在完美的数学模型;2)系统的扰动是不可控的,也很难建模的;3)测量的传感器本身存在着误差。而影响我们这种误差的称为噪声,例如我们在使用PNP测距的时候,远距离是非常不精确的,它的波动可能会很大,这个时候我们就需要进行一下滤波,如下图所示(蓝色为滤波后数据、红色为实际测量值、绿色为理想测量值)
在我的理解里,卡尔曼滤波就相当于一个带有权重的低通滤波,即调整观测值的权重(测量值)和估计值的权重,是更相信观测值还是更相信估计值的一个过程,观测值*权重+估计值*权重 = 修正值,即最优估计。
Kalman滤波是一种递归过程,主要两个更新过程:时间更新和观测更新,其中时间更新主要包括状态预测和协方差预测,主要是对系统的预测,而观测更新主要包括计算卡尔曼增益、状态更新和协方差更新,因此整个递归过程主要包括五个方面的计算:
1、状态预测;
2、协方差预测;
3、卡尔曼增益
4、状态更新(修正);
5、协方差更新(修正);
卡尔曼滤波的适用条件主要有:
1、应用系统必须是线性的;
2、对测量造成影响的噪声必须是符合高斯分布的白噪声。
白噪声:是一种功率谱密度为常数的随机信号或随机过程。即此信号在各个频段上的功率一致。其余的统称为有色噪声
03 卡尔曼公式推导及理解
3.1 基本模型
在介绍卡尔曼公式理解前先介绍一下状态空间方程,便于后面的公式理解。
状态方程:
x
k
=
A
x
k
−
1
+
B
u
k
−
1
+
ω
k
−
1
x_k = Ax_{k-1} + Bu_{k-1} + \omega_{k-1}
xk=Axk−1+Buk−1+ωk−1
观测方程:
z
k
=
H
x
k
+
υ
k
z_k = Hx_k + \upsilon_k
zk=Hxk+υk
x k x_k xk:当前 k k k时刻的系统状态
x k − 1 x_{k-1} xk−1: k − 1 k-1 k−1时刻的系统状态
A A A:传输参数
B B B:控制参数
u k − 1 u_{k-1} uk−1: k − 1 k-1 k−1对系统的控制量
z k z_k zk: k k k时刻的测量值或观测值
H H H:状态转移矩阵
ω k − 1 \omega_{k-1} ωk−1和 υ k \upsilon_k υk:分别为 k − 1 k-1 k−1时刻的过程噪声和 k k k时刻的观测噪声。
过程噪声和观测噪声假设是符合高斯分布,即:
p
(
ω
k
)
∼
N
(
0
,
Q
)
p
(
υ
k
)
∼
N
(
0
,
R
)
p(\omega_k) \sim N(0, Q)\\ p(\upsilon_k) \sim N(0, R)
p(ωk)∼N(0,Q)p(υk)∼N(0,R)
ω k \omega_k ωk:过程噪声
υ k \upsilon_k υk:观测噪声
Q Q Q:过程噪声协方差矩阵
R R R:观测噪声协方差矩阵
3.2卡尔曼公式
时间更新:
x
^
k
−
=
A
x
^
k
−
1
+
B
u
k
−
1
P
k
−
=
A
P
k
−
1
A
T
+
Q
\hat{x}^{-}_k = A\hat{x}_{k-1}+Bu_{k-1}\\ P^-_k = AP_{k-1}A^{T} + Q
x^k−=Ax^k−1+Buk−1Pk−=APk−1AT+Q
状态更新:
K
k
=
P
k
−
H
T
(
H
P
k
−
H
T
+
R
)
−
1
x
^
k
=
x
^
k
−
+
K
k
(
z
k
−
H
x
^
k
−
)
P
k
=
(
I
−
K
k
H
)
P
k
−
K_k = P^-_kH^{T}(HP^-_kH^{T} + R)^{-1} \\ \hat{x}_k = \hat{x}^-_k + K_k(z_k - H\hat{x}^-_k)\\ P_k = (I - K_kH)P^-_k
Kk=Pk−HT(HPk−HT+R)−1x^k=x^k−+Kk(zk−Hx^k−)Pk=(I−KkH)Pk−
x ^ k − \hat{x}^-_k x^k−:当前状态先验估计值
x ^ k − 1 \hat{x}_{k-1} x^k−1:上一状态的最优估计
u k − 1 u_{k-1} uk−1:上一状态的系统控制量
P k − P^-_k Pk−:当前状态先验估计值的协方差
P k − 1 P_{k-1} Pk−1:上一状态的协方差修正值
A A A:状态转移矩阵
B B B:控制矩阵
Q Q Q:过程噪声的方差
K k K_k Kk:当前状态的卡尔曼增益(Kalman Gain)
H H H:观测系统的参数
R R R:测量噪声的方差
I I I:单位矩阵
协方差( c o v ( X , Y ) cov(X, Y) cov(X,Y))可以理解为 X 、 Y X、Y X、Y两个变量的数值上的相关性,即正相关还是负相关
均方误差:误差的平方的期望值,也就是多个样本的时候,均分误差等于每个样本的误差的平方乘以该样本出现的概率和
方差:方差是描述随机变量的离散程度,是变量离期望值的距离
两个变量间的协方差:
c o v ( X , Y ) = E [ ( X − E ( X ) ) ( Y − E ( Y ) ) ] = E ( X Y ) − E ( X ) E ( Y ) cov(X, Y) = E[(X-E(X))(Y-E(Y))] = E(XY) - E(X)E(Y) cov(X,Y)=E[(X−E(X))(Y−E(Y))]=E(XY)−E(X)E(Y)
它表示两个变量之间的总体误差,当 Y = X Y=X Y=X的时候,就是方差 D ( X ) = D ( Y ) D(X) = D(Y) D(X)=D(Y)。
我们将均值去掉
( X − E ( X ) ) ( Y − E ( Y ) ) (X-E(X))(Y-E(Y)) (X−E(X))(Y−E(Y))
就成了两个公式相乘,即当样本数据 X X X大于自身期望, Y Y Y也大于自身期望时,那么协方差就为正值,为正相关,反之为负相关。
在现实生活中,我们常常会遇到多维的数据,方差是不足以满足我们的需求的,所以我们要引入协方差来处理多维的数据。
通俗理解就是使用上一个状态的最优估计(后验估计)来得出当前的先验估计,在由当前的观测值来修正得到最优估计,即后验估计的一个递归过程。
3.3卡尔曼公式推导
时间更新公式:
x
^
k
−
=
A
x
^
k
−
1
+
B
u
k
P
k
−
=
A
P
k
−
1
A
T
+
Q
\hat{x}^{-}_k = A\hat{x}_{k-1}+Bu_{k}\\ P^-_k = AP_{k-1}A^{T} + Q
x^k−=Ax^k−1+BukPk−=APk−1AT+Q
其中
x
^
k
−
=
F
x
^
k
−
1
+
B
u
k
\hat{x}^{-}_k = F\hat{x}_{k-1}+Bu_{k}
x^k−=Fx^k−1+Buk为状态空间方程去除过程噪声得到的先验估计推导公式
关于
P
k
−
=
A
P
k
−
1
A
T
+
Q
P^-_k = AP_{k-1}A^{T} + Q
Pk−=APk−1AT+Q
我们知道
c
o
v
(
a
X
+
B
,
a
X
+
B
)
=
a
c
o
v
(
X
,
X
)
a
T
cov(aX+B,aX+B)= acov(X,X)a^T
cov(aX+B,aX+B)=acov(X,X)aT
那么
c
o
v
(
x
^
k
−
,
x
^
k
−
)
=
c
o
v
(
A
x
^
k
−
1
+
B
u
k
,
A
x
^
k
−
1
+
B
u
k
)
=
A
c
o
v
(
x
^
k
−
1
,
x
^
k
−
1
)
A
T
+
Q
cov(\hat{x}^{-}_k,\hat{x}^{-}_k) = cov(A\hat{x}_{k-1}+Bu_{k},A\hat{x}_{k-1}+Bu_{k}) \\ = Acov(\hat{x}_{k-1},\hat{x}_{k-1})A^T + Q
cov(x^k−,x^k−)=cov(Ax^k−1+Buk,Ax^k−1+Buk)=Acov(x^k−1,x^k−1)AT+Q
这个
Q
Q
Q是过程噪声的协方差,即
c
o
v
(
ω
k
,
ω
k
)
cov(\omega_k,\omega_k)
cov(ωk,ωk)
故
P
k
−
=
c
o
v
(
x
^
k
−
,
x
^
k
−
)
=
A
c
o
v
(
x
^
k
−
1
,
x
^
k
−
1
)
A
T
+
Q
=
A
P
k
−
1
A
T
+
Q
P^-_k = cov(\hat{x}^{-}_k,\hat{x}^{-}_k) =Acov(\hat{x}_{k-1},\hat{x}_{k-1})A^T + Q = AP_{k-1}A^{T} + Q
Pk−=cov(x^k−,x^k−)=Acov(x^k−1,x^k−1)AT+Q=APk−1AT+Q
状态更新公式:
K
k
=
P
k
−
H
T
(
H
P
k
−
H
T
+
R
)
−
1
x
^
k
=
x
^
k
−
+
K
k
(
z
k
−
H
x
^
k
−
)
P
k
=
(
I
−
K
k
H
)
P
k
−
K_k = P^-_kH^{T}(HP^-_kH^{T} + R)^{-1} \\ \hat{x}_k = \hat{x}^-_k + K_k(z_k - H\hat{x}^-_k)\\ P_k = (I - K_kH)P^-_k
Kk=Pk−HT(HPk−HT+R)−1x^k=x^k−+Kk(zk−Hx^k−)Pk=(I−KkH)Pk−
关于
x
^
k
=
x
^
k
−
+
K
k
(
z
k
−
H
x
^
k
−
)
\hat{x}_k = \hat{x}^-_k + K_k(z_k - H\hat{x}^-_k)
x^k=x^k−+Kk(zk−Hx^k−),我们可以通过一个一维的例子来对该公式进行一个引入,例如:
我们需要测量一个物体的长度,通常我们都会通过多次测量进而取平均值,即
x
^
k
=
1
k
(
z
1
+
z
2
+
z
3
+
.
.
.
+
z
k
)
=
1
k
(
z
1
+
z
2
+
z
3
+
.
.
.
+
z
k
−
1
)
+
1
k
z
k
=
1
k
k
−
1
k
−
1
(
z
1
+
z
2
+
z
3
+
.
.
.
+
z
k
−
1
)
+
1
k
z
k
=
k
−
1
k
x
^
k
−
1
+
1
k
z
k
=
x
^
k
−
1
−
1
k
x
^
k
−
1
+
1
k
z
k
\begin{aligned} \hat{x}_k &= \frac{1}{k}(z_1+z_2+z_3+...+z_k)\\ &= \frac{1}{k}(z_1+z_2+z_3+...+z_{k-1}) + \frac{1}{k}z_k\\ &= \frac{1}{k} \frac{k-1}{k-1}(z_1+z_2+z_3+...+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}\hat{x}_{k-1}+\frac{1}{k}z_k \end{aligned}
x^k=k1(z1+z2+z3+...+zk)=k1(z1+z2+z3+...+zk−1)+k1zk=k1k−1k−1(z1+z2+z3+...+zk−1)+k1zk=kk−1x^k−1+k1zk=x^k−1−k1x^k−1+k1zk
所以
x
^
k
=
x
^
k
−
1
+
1
k
(
z
k
−
x
^
k
−
1
)
\hat{x}_k = \hat{x}_{k-1}+\frac{1}{k}(z_k-\hat{x}_{k-1})
x^k=x^k−1+k1(zk−x^k−1)
我们将
1
k
=
K
k
\frac{1}{k} = K_k
k1=Kk
则
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^k−1+Kk(zk−x^k−1)
即当前的估计值 = 上一次的估计值+系数x(当前的测量值-上一次的估计值)
而在卡尔曼滤波中,我们将这个系数
K
k
K_k
Kk称为卡尔曼增益(Kalman Gain)
而在多维的情况下已知状态空间方程:
x
k
=
A
x
k
−
1
+
B
u
k
−
1
+
ω
k
−
1
z
k
=
H
x
k
+
υ
k
x_k = Ax_{k-1} + Bu_{k-1} + \omega_{k-1}\\ z_k = Hx_k + \upsilon_k
xk=Axk−1+Buk−1+ωk−1zk=Hxk+υk
而这个
ω
k
−
1
\omega_{k-1}
ωk−1和
υ
k
\upsilon_k
υk是我们没办法建模的,所以,我们只取前面部分作为估计值
即
x
^
k
−
=
A
x
^
k
−
1
+
B
u
k
−
1
z
k
=
H
x
k
\hat{x}^-_k = A\hat{x}_{k-1} + Bu_{k-1}\\ z_k = Hx_k
x^k−=Ax^k−1+Buk−1zk=Hxk
而
z
k
=
H
x
k
⇒
x
k
=
H
−
z
k
z_k = Hx_k \Rightarrow x_k = H^-z_k
zk=Hxk⇒xk=H−zk
由刚刚的当前的值
x
^
k
=
\hat{x}_k=
x^k=算出来的结果
x
^
−
\hat{x}^-
x^−+系数
G
∗
(
G*(
G∗(测出来的结果
H
−
z
k
−
H^-z_k-
H−zk−算出来的结果
x
k
−
)
x^-_k)
xk−)
所以有
x
^
k
=
x
^
k
−
+
G
(
H
−
z
k
−
x
^
k
−
)
\hat{x}_k = \hat{x}^-_k+G(H^-z_k- \hat{x}^-_k)
x^k=x^k−+G(H−zk−x^k−)
即当
G
=
0
G = 0
G=0的时候,
x
^
k
=
x
^
k
−
\hat{x}_k = \hat{x}^-_k
x^k=x^k−,即相信估计值,当
G
=
1
G = 1
G=1的时候,
x
^
k
=
H
−
z
k
\hat{x}_k = H^-z_k
x^k=H−zk,即更相信测量值。
而在卡尔曼滤波中,
G
=
K
k
H
G = K_kH
G=KkH,代入就可以得到:
x
^
k
=
x
^
k
−
+
K
k
(
z
k
−
H
x
^
k
−
)
\hat{x}_k = \hat{x}^-_k + K_k(z_k - H\hat{x}^-_k)
x^k=x^k−+Kk(zk−Hx^k−)
所以当
K
k
=
0
K_k = 0
Kk=0时,
x
^
k
=
x
^
k
−
\hat{x}_k=\hat{x}^-_k
x^k=x^k−;当
K
k
=
H
−
K_k = H^-
Kk=H−时,
x
^
k
=
H
−
z
k
\hat{x}_k = H^-z_k
x^k=H−zk。
这里我们引入真实值与估计值的误差
e
k
e_k
ek和真实值与先验估计的误差:
e
k
=
x
k
−
x
^
k
e
k
−
=
x
k
−
x
^
k
−
e_k = x_k - \hat{x}_k\\ e_k^-=x_k-\hat{x}^-_k
ek=xk−x^kek−=xk−x^k−
我们假设这个误差也符合高斯分布,即
p
(
e
k
)
∼
N
(
0
,
P
)
p(e_k)\sim N(0,P)
p(ek)∼N(0,P),这里的协方差矩阵
P
=
E
[
e
e
T
]
P = E[ee^T]
P=E[eeT]
重点:如果我们新估计的结果 x ^ k \hat{x}_k x^k与实际值 x k x_k xk最小呢,也就是说它整个的误差的方差最小,方差越小,那么它的期望值越接近于0,所以我们需要选取一个合适的值,使得它的协方差矩阵 P P P的迹 t r ( P ) tr(P) tr(P)最小。
已知
e
k
=
x
k
−
x
^
k
e_k = x_k - \hat{x}_k
ek=xk−x^k,那么
P
k
=
E
[
e
k
e
k
T
]
=
E
[
(
x
k
−
x
^
k
)
(
x
k
−
x
^
k
)
T
]
P_k = E[e_ke^T_k] = E[(x_k - \hat{x}_k)(x_k - \hat{x}_k)^T]
Pk=E[ekekT]=E[(xk−x^k)(xk−x^k)T]
因为我们要求
K
k
K_k
Kk,那么我们将
x
^
k
=
x
^
k
−
+
K
k
(
z
k
−
H
x
^
k
−
)
\hat{x}_k = \hat{x}^-_k + K_k(z_k - H\hat{x}^-_k)
x^k=x^k−+Kk(zk−Hx^k−)代入上式中
先化简一下
x
k
−
x
^
k
x_k-\hat{x}_k
xk−x^k:
x
k
−
x
^
k
=
x
k
−
(
x
^
k
−
+
K
k
(
z
k
−
H
x
^
k
−
)
)
=
x
k
−
x
^
k
−
−
K
k
z
k
+
K
k
H
x
^
k
−
=
x
k
−
x
^
k
−
−
K
k
H
x
k
−
K
k
v
k
+
K
k
H
x
^
k
−
=
(
x
k
−
x
^
k
−
)
−
K
k
H
(
x
k
−
x
^
k
−
)
−
K
k
v
k
=
(
I
−
K
k
H
)
(
x
k
−
x
^
k
−
)
−
K
k
v
k
=
(
I
−
K
k
H
)
e
k
−
−
K
k
v
k
\begin{aligned} x_k-\hat{x}_k &= x_k-(\hat{x}^-_k + K_k(z_k - H\hat{x}^-_k))\\ &=x_k-\hat{x}^-_k-K_kz_k+K_kH\hat{x}^-_k\\ &=x_k-\hat{x}^-_k-K_kHx_k-K_kv_k+K_kH\hat{x}^-_k\\ &=(x_k-\hat{x}^-_k)-K_kH(x_k-\hat{x}^-_k)-K_kv_k\\ &=(I-K_kH)(x_k-\hat{x}^-_k)-K_kv_k\\ &=(I-K_kH)e^-_k-K_kv_k \end{aligned}
xk−x^k=xk−(x^k−+Kk(zk−Hx^k−))=xk−x^k−−Kkzk+KkHx^k−=xk−x^k−−KkHxk−Kkvk+KkHx^k−=(xk−x^k−)−KkH(xk−x^k−)−Kkvk=(I−KkH)(xk−x^k−)−Kkvk=(I−KkH)ek−−Kkvk
那么
P
k
=
E
[
(
x
k
−
x
^
k
)
(
x
k
−
x
^
k
)
T
]
=
E
[
[
(
I
−
K
k
H
)
e
k
−
−
K
k
v
k
]
[
(
I
−
K
k
H
)
e
k
−
−
K
k
v
k
]
T
]
=
E
[
[
(
I
−
K
k
H
)
e
k
−
−
K
k
v
k
]
[
e
k
−
T
(
I
−
K
k
H
)
T
−
v
k
T
K
k
T
]
]
=
E
[
(
I
−
K
k
H
)
e
k
−
e
k
−
T
(
I
−
K
k
H
)
T
−
(
I
−
K
k
H
)
e
k
−
v
k
T
K
k
T
−
K
k
v
k
[
e
k
−
T
(
I
−
K
k
H
)
T
+
K
k
v
k
v
k
T
K
k
T
]
=
E
[
(
I
−
K
k
H
)
e
k
−
e
k
−
T
(
I
−
K
k
H
)
T
]
−
E
[
(
I
−
K
k
H
)
e
k
−
v
k
T
K
k
T
]
−
E
[
K
k
v
k
[
e
k
−
T
(
I
−
K
k
H
)
T
]
+
E
[
K
k
v
k
v
k
T
K
k
T
]
\begin{aligned} P_k &= E[(x_k - \hat{x}_k)(x_k - \hat{x}_k)^T]\\ &=E[[(I-K_kH)e^-_k-K_kv_k][(I-K_kH)e^-_k-K_kv_k]^T]\\ &=E[[(I-K_kH)e^-_k-K_kv_k][e^{-T}_k(I-K_kH)^T-v_k^TK_k^T]]\\ &=E[(I-K_kH)e^-_ke^{-T}_k(I-K_kH)^T-(I-K_kH)e^-_kv_k^TK_k^T-K_kv_k[e^{-T}_k(I-K_kH)^T+K_kv_kv_k^TK_k^T]\\ &=E[(I-K_kH)e^-_ke^{-T}_k(I-K_kH)^T]-E[(I-K_kH)e^-_kv_k^TK_k^T]-E[K_kv_k[e^{-T}_k(I-K_kH)^T]+E[K_kv_kv_k^TK_k^T] \end{aligned}
Pk=E[(xk−x^k)(xk−x^k)T]=E[[(I−KkH)ek−−Kkvk][(I−KkH)ek−−Kkvk]T]=E[[(I−KkH)ek−−Kkvk][ek−T(I−KkH)T−vkTKkT]]=E[(I−KkH)ek−ek−T(I−KkH)T−(I−KkH)ek−vkTKkT−Kkvk[ek−T(I−KkH)T+KkvkvkTKkT]=E[(I−KkH)ek−ek−T(I−KkH)T]−E[(I−KkH)ek−vkTKkT]−E[Kkvk[ek−T(I−KkH)T]+E[KkvkvkTKkT]
第二项
E
[
(
I
−
K
k
H
)
e
k
−
v
k
T
K
k
T
]
=
(
I
−
K
k
H
)
E
(
e
k
−
v
k
T
)
K
k
T
E[(I-K_kH)e^-_kv_k^TK_k^T] = (I-K_kH)E(e^-_kv^T_k)K^T_k
E[(I−KkH)ek−vkTKkT]=(I−KkH)E(ek−vkT)KkT
因为
e
k
−
e^-_k
ek−与
v
k
T
v^T_k
vkT是相互独立的,即先验误差和测量误差是没有什么关系的,所以
E
(
e
k
−
v
k
T
)
=
E
(
e
k
−
)
E
(
v
k
T
)
E(e^-_kv^T_k) = E(e^-_k)E(v^T_k)
E(ek−vkT)=E(ek−)E(vkT)
又因为
p
(
v
k
T
)
∼
N
(
0
,
R
k
)
p(v^T_k)\sim N(0, R_k)
p(vkT)∼N(0,Rk)以及
p
(
e
k
−
)
∼
N
(
0
,
P
)
p(e^-_k)\sim N(0,P)
p(ek−)∼N(0,P),所以
E
(
e
k
−
)
=
E
(
v
k
T
)
=
0
E(e^-_k) = E(v^T_k) = 0
E(ek−)=E(vkT)=0
所以
E
[
(
I
−
K
k
H
)
e
k
−
v
k
T
K
k
T
]
=
0
E[(I-K_kH)e^-_kv_k^TK_k^T] = 0
E[(I−KkH)ek−vkTKkT]=0
同理第三项
E
[
K
k
v
k
[
e
k
−
T
(
I
−
K
k
H
)
T
]
]
=
K
k
E
(
v
k
)
E
(
e
k
−
T
)
(
I
−
K
k
H
)
T
=
0
E[K_kv_k[e^{-T}_k(I-K_kH)^T]] = K_kE(v_k)E(e^{-T}_k)(I-K_kH)^T = 0
E[Kkvk[ek−T(I−KkH)T]]=KkE(vk)E(ek−T)(I−KkH)T=0
故
P
k
=
E
[
(
x
k
−
x
^
k
)
(
x
k
−
x
^
k
)
T
]
=
E
[
[
(
I
−
K
k
H
)
e
k
−
−
K
k
v
k
]
[
(
I
−
K
k
H
)
e
k
−
−
K
k
v
k
]
T
]
=
E
[
[
(
I
−
K
k
H
)
e
k
−
−
K
k
v
k
]
[
e
k
−
T
(
I
−
K
k
H
)
T
−
v
k
T
K
k
T
]
]
=
E
[
(
I
−
K
k
H
)
e
k
−
e
k
−
T
(
I
−
K
k
H
)
T
−
(
I
−
K
k
H
)
e
k
−
v
k
T
K
k
T
−
K
k
v
k
[
e
k
−
T
(
I
−
K
k
H
)
T
+
K
k
v
k
v
k
T
K
k
T
]
=
E
[
(
I
−
K
k
H
)
e
k
−
e
k
−
T
(
I
−
K
k
H
)
T
]
−
E
[
(
I
−
K
k
H
)
e
k
−
v
k
T
K
k
T
]
−
E
[
K
k
v
k
[
e
k
−
T
(
I
−
K
k
H
)
T
]
+
E
[
K
k
v
k
v
k
T
K
k
T
]
=
E
[
(
I
−
K
k
H
)
e
k
−
e
k
−
T
(
I
−
K
k
H
)
T
]
+
E
[
K
k
v
k
v
k
T
K
k
T
]
=
(
I
−
K
k
H
)
E
(
e
k
−
e
k
−
T
)
(
I
−
K
k
H
)
T
+
K
k
E
(
v
k
v
k
T
)
K
k
T
\begin{aligned} P_k &= E[(x_k - \hat{x}_k)(x_k - \hat{x}_k)^T]\\ &=E[[(I-K_kH)e^-_k-K_kv_k][(I-K_kH)e^-_k-K_kv_k]^T]\\ &=E[[(I-K_kH)e^-_k-K_kv_k][e^{-T}_k(I-K_kH)^T-v_k^TK_k^T]]\\ &=E[(I-K_kH)e^-_ke^{-T}_k(I-K_kH)^T-(I-K_kH)e^-_kv_k^TK_k^T-K_kv_k[e^{-T}_k(I-K_kH)^T+K_kv_kv_k^TK_k^T]\\ &=E[(I-K_kH)e^-_ke^{-T}_k(I-K_kH)^T]-E[(I-K_kH)e^-_kv_k^TK_k^T]-E[K_kv_k[e^{-T}_k(I-K_kH)^T]+E[K_kv_kv_k^TK_k^T]\\ &=E[(I-K_kH)e^-_ke^{-T}_k(I-K_kH)^T]+E[K_kv_kv_k^TK_k^T]\\ &=(I-K_kH)E(e^-_ke^{-T}_k)(I-K_kH)^T+K_kE(v_kv^T_k)K^T_k \end{aligned}
Pk=E[(xk−x^k)(xk−x^k)T]=E[[(I−KkH)ek−−Kkvk][(I−KkH)ek−−Kkvk]T]=E[[(I−KkH)ek−−Kkvk][ek−T(I−KkH)T−vkTKkT]]=E[(I−KkH)ek−ek−T(I−KkH)T−(I−KkH)ek−vkTKkT−Kkvk[ek−T(I−KkH)T+KkvkvkTKkT]=E[(I−KkH)ek−ek−T(I−KkH)T]−E[(I−KkH)ek−vkTKkT]−E[Kkvk[ek−T(I−KkH)T]+E[KkvkvkTKkT]=E[(I−KkH)ek−ek−T(I−KkH)T]+E[KkvkvkTKkT]=(I−KkH)E(ek−ek−T)(I−KkH)T+KkE(vkvkT)KkT
由于
P
k
=
E
(
e
k
e
k
T
)
P_k=E(e_ke^T_k)
Pk=E(ekekT),则
E
(
e
k
−
e
k
−
T
)
=
P
k
−
E(e^-_ke^{-T}_k) = P^-_k
E(ek−ek−T)=Pk−
所以代入得
P
k
=
(
I
−
K
k
H
)
P
k
−
(
I
−
K
k
H
)
T
+
K
k
E
(
v
k
v
k
T
)
K
k
T
=
(
I
P
k
−
−
K
k
H
P
k
−
)
(
I
T
−
H
T
K
k
T
)
+
K
k
E
(
v
k
v
k
T
)
K
k
T
\begin{aligned} P_k &= (I-K_kH)P^-_k(I-K_kH)^T+K_kE(v_kv^T_k)K^T_k\\ &=(IP^-_k-K_kHP^-_k)(I^T-H^TK^T_k)+K_kE(v_kv^T_k)K^T_k \end{aligned}
Pk=(I−KkH)Pk−(I−KkH)T+KkE(vkvkT)KkT=(IPk−−KkHPk−)(IT−HTKkT)+KkE(vkvkT)KkT
又因为
p
(
v
k
)
∼
N
(
0
,
R
k
)
p(v_k)\sim N(0, R_k)
p(vk)∼N(0,Rk),即
E
(
v
k
v
k
T
)
=
R
k
E(v_kv^T_k) = R_k
E(vkvkT)=Rk,所以
P
k
=
(
I
−
K
k
H
)
P
k
−
(
I
−
K
k
H
)
T
+
K
k
E
(
v
k
v
k
T
)
K
k
T
=
(
P
k
−
−
K
k
H
P
k
−
)
(
I
T
−
H
T
K
k
T
)
+
K
k
E
(
v
k
v
k
T
)
K
k
T
=
(
P
k
−
−
K
k
H
P
k
−
)
(
I
T
−
H
T
K
k
T
)
+
K
k
R
K
k
T
=
P
k
−
−
K
k
H
P
k
−
−
P
k
−
H
T
K
k
T
+
K
k
H
P
k
−
H
T
K
k
T
+
K
k
R
K
k
T
\begin{aligned} P_k &= (I-K_kH)P^-_k(I-K_kH)^T+K_kE(v_kv^T_k)K^T_k\\ &=(P^-_k-K_kHP^-_k)(I^T-H^TK^T_k)+K_kE(v_kv^T_k)K^T_k\\ &=(P^-_k-K_kHP^-_k)(I^T-H^TK^T_k)+K_kRK^T_k\\ &=P^-_k-K_kHP^-_k-P^-_kH^TK^T_k+K_kHP^-_kH^TK^T_k+K_kRK^T_k \end{aligned}
Pk=(I−KkH)Pk−(I−KkH)T+KkE(vkvkT)KkT=(Pk−−KkHPk−)(IT−HTKkT)+KkE(vkvkT)KkT=(Pk−−KkHPk−)(IT−HTKkT)+KkRKkT=Pk−−KkHPk−−Pk−HTKkT+KkHPk−HTKkT+KkRKkT
我们要求的是
t
r
(
P
k
)
tr(P_k)
tr(Pk),因为第三项的转置
(
P
k
−
H
T
K
k
T
)
T
=
K
k
(
P
k
−
H
T
)
T
=
K
k
H
P
k
−
T
(P^-_kH^TK^T_k)^T = K_k(P^-_kH^T)^T = K_kHP^{-T}_k
(Pk−HTKkT)T=Kk(Pk−HT)T=KkHPk−T
又因为协方差矩阵是对称矩阵,所以
P
k
−
T
=
P
k
−
P^{-T}_k=P^-_k
Pk−T=Pk−,所以第二项和第三项互为转置,又因为互为转置,那么对角线上的元素是一样的,故
t
r
(
K
k
H
P
k
−
)
=
t
r
(
P
k
−
H
T
K
k
T
)
tr(K_kHP^-_k)=tr(P^-_kH^TK^T_k)
tr(KkHPk−)=tr(Pk−HTKkT),所以
t
r
(
P
k
)
=
t
r
(
P
k
−
)
−
2
t
r
(
K
k
H
P
k
−
)
+
t
r
(
K
k
H
P
k
−
H
T
K
k
T
)
+
t
r
(
K
k
R
K
k
T
)
tr(P_k) = tr(P^-_k)-2tr(K_kHP^-_k)+tr(K_kHP^-_kH^TK^T_k)+tr(K_kRK^T_k)
tr(Pk)=tr(Pk−)−2tr(KkHPk−)+tr(KkHPk−HTKkT)+tr(KkRKkT)
由于目标是寻找
K
k
K_k
Kk使得
t
r
(
P
k
)
tr(P_k)
tr(Pk)有最小值,所以我们令
t
r
(
P
k
)
tr(P_k)
tr(Pk)对
K
k
K_k
Kk求导,令其等于0,即
令
d
t
r
(
P
k
)
d
K
k
=
0
令\frac{dtr(P_k)}{dK_k} = 0
令dKkdtr(Pk)=0
所以
d
t
r
(
P
k
)
d
K
k
=
0
−
d
2
t
r
(
K
k
H
P
k
−
)
d
K
k
+
d
t
r
(
K
k
H
P
k
−
H
T
K
k
T
)
d
K
k
+
d
t
r
(
K
k
R
K
k
T
)
d
K
k
\frac{dtr(P_k)}{dK_k} = 0-\frac{d2tr(K_kHP^-_k)}{dK_k}+\frac{dtr(K_kHP^-_kH^TK^T_k)}{dK_k}+\frac{dtr(K_kRK^T_k)}{dK_k}
dKkdtr(Pk)=0−dKkd2tr(KkHPk−)+dKkdtr(KkHPk−HTKkT)+dKkdtr(KkRKkT)
这里需要一个用到两个矩阵微分公式
公式一:
d t r ( A B ) d A = B T \frac{dtr(AB)}{dA} = B^T dAdtr(AB)=BT
公式二:
d t r ( A B A T ) d A = 2 A B \frac{dtr(ABA^T)}{dA} = 2AB dAdtr(ABAT)=2AB
所以
d
t
r
(
P
k
)
d
K
k
=
0
−
2
(
H
P
k
−
)
T
+
2
K
k
H
P
k
−
H
T
+
2
K
k
R
\frac{dtr(P_k)}{dK_k} =0-2(HP^-_k)^T+2K_kHP^-_kH^T+2K_kR
dKkdtr(Pk)=0−2(HPk−)T+2KkHPk−HT+2KkR
令其为0
d
t
r
(
P
k
)
d
K
k
=
−
2
(
H
P
k
−
)
T
+
2
K
k
H
P
k
−
H
T
+
2
K
k
R
=
0
⇒
−
(
H
P
k
−
)
T
+
K
k
H
P
k
−
H
T
+
K
k
R
=
0
⇒
−
P
k
−
H
T
+
K
k
(
H
P
k
−
H
T
+
R
)
=
0
⇒
K
k
(
H
P
k
−
H
T
+
R
)
=
P
k
−
H
T
\frac{dtr(P_k)}{dK_k} =-2(HP^-_k)^T+2K_kHP^-_kH^T+2K_kR=0\\ \Rightarrow-(HP^-_k)^T+ K_kHP^-_kH^T+ K_kR=0\\ \Rightarrow -P^-_kH^T+K_k(HP^-_kH^T+R)=0\\ \Rightarrow K_k(HP^-_kH^T+R) = P^-_kH^T
dKkdtr(Pk)=−2(HPk−)T+2KkHPk−HT+2KkR=0⇒−(HPk−)T+KkHPk−HT+KkR=0⇒−Pk−HT+Kk(HPk−HT+R)=0⇒Kk(HPk−HT+R)=Pk−HT
所以
K
k
=
P
k
−
H
T
H
P
k
−
H
T
+
R
K_k = \frac{P^-_kH^T}{HP^-_kH^T+R}
Kk=HPk−HT+RPk−HT
由上面可知
R
=
E
(
v
k
v
k
T
)
R = E(v_kv^T_k)
R=E(vkvkT)
所以当
R
R
R特别大的时候,即测量噪声特别大时,
K
k
→
0
K_k\rightarrow 0
Kk→0,这时
x
^
k
=
x
^
k
−
\hat{x}_k = \hat{x}^-_k
x^k=x^k−;
当
R
R
R特别小的时候,即
R
→
0
R\rightarrow0
R→0,这时
K
k
→
H
−
K_k\rightarrow H^-
Kk→H−,这时
x
^
k
=
H
−
z
k
\hat{x}_k = H^-z_k
x^k=H−zk。
由上面的时间更新的推导,已知
P
k
−
=
c
o
v
(
x
^
k
−
,
x
^
k
−
)
=
A
c
o
v
(
x
^
k
−
1
,
x
^
k
−
1
)
A
T
+
Q
=
A
P
k
−
1
A
T
+
Q
P^-_k = cov(\hat{x}^{-}_k,\hat{x}^{-}_k) =Acov(\hat{x}_{k-1},\hat{x}_{k-1})A^T + Q = AP_{k-1}A^{T} + Q
Pk−=cov(x^k−,x^k−)=Acov(x^k−1,x^k−1)AT+Q=APk−1AT+Q
即
P
k
−
=
A
P
k
−
1
A
T
+
Q
P^-_k = AP_{k-1}A^{T} + Q
Pk−=APk−1AT+Q
下面再推导一下 P k P_k Pk的修正公式
由刚刚推导
K
k
K_k
Kk时的结论可知
P
k
=
P
k
−
−
K
k
H
P
k
−
−
P
k
−
H
T
K
k
T
+
K
k
H
P
k
−
H
T
K
k
T
+
K
k
R
K
k
T
=
P
k
−
−
K
k
H
P
k
−
−
P
k
−
H
T
K
k
T
+
K
k
(
H
P
k
−
K
k
T
+
R
)
K
k
T
\begin{aligned} P_k &= P^-_k-K_kHP^-_k-P^-_kH^TK^T_k+K_kHP^-_kH^TK^T_k+K_kRK^T_k\\ &=P^-_k-K_kHP^-_k-P^-_kH^TK^T_k+K_k(HP^-_kK^T_k + R)K^T_k \end{aligned}
Pk=Pk−−KkHPk−−Pk−HTKkT+KkHPk−HTKkT+KkRKkT=Pk−−KkHPk−−Pk−HTKkT+Kk(HPk−KkT+R)KkT
我们将
K
k
=
P
k
−
H
T
H
P
k
−
H
T
+
R
K_k = \frac{P^-_kH^T}{HP^-_kH^T+R}
Kk=HPk−HT+RPk−HT
代入上式
P
k
=
P
k
−
−
K
k
H
P
k
−
−
P
k
−
H
T
K
k
T
+
P
k
−
H
T
H
P
k
−
H
T
+
R
(
H
P
k
−
K
k
T
+
R
)
K
k
T
=
P
k
−
−
K
k
H
P
k
−
−
P
k
−
H
T
K
k
T
+
P
k
−
H
T
K
k
T
=
P
k
−
−
K
k
H
P
k
−
=
(
I
−
K
k
H
)
P
k
−
\begin{aligned} P_k &= P^-_k-K_kHP^-_k-P^-_kH^TK^T_k+\frac{P^-_kH^T}{HP^-_kH^T+R}(HP^-_kK^T_k + R)K^T_k\\ &=P^-_k-K_kHP^-_k-P^-_kH^TK^T_k+P^-_kH^TK^T_k\\ &=P^-_k-K_kHP^-_k\\ &=(I-K_kH)P^-_k \end{aligned}
Pk=Pk−−KkHPk−−Pk−HTKkT+HPk−HT+RPk−HT(HPk−KkT+R)KkT=Pk−−KkHPk−−Pk−HTKkT+Pk−HTKkT=Pk−−KkHPk−=(I−KkH)Pk−
故
P
k
=
(
I
−
K
k
H
)
P
k
−
P_k = (I-K_kH)P^-_k
Pk=(I−KkH)Pk−
到现在我们卡尔曼滤波的五个公式就全部推到完成了
时间更新 | 状态更新 |
---|---|
x ^ k − = A x ^ k − 1 + B u k − 1 \hat{x}^-_k=A\hat{x}_{k-1}+Bu_{k-1} x^k−=Ax^k−1+Buk−1 | K k = P k − H T ( H P k − H T + R ) − 1 K_k = P^-_kH^{T}(HP^-_kH^{T} + R)^{-1} Kk=Pk−HT(HPk−HT+R)−1 |
P k − = A P k − 1 A T + Q P^-_k=AP_{k-1}A^T+Q Pk−=APk−1AT+Q | x ^ k = x ^ k − + K k ( z k − H x ^ k − ) \hat{x}_k = \hat{x}^-_k + K_k(z_k - H\hat{x}^-_k) x^k=x^k−+Kk(zk−Hx^k−) |
P k = ( I − K k H ) P k − P_k = (I - K_kH)P^-_k Pk=(I−KkH)Pk− |
04卡尔曼滤波超参数调节
已知
P
k
−
=
A
P
k
−
1
A
T
+
Q
K
k
=
P
k
−
H
T
(
H
P
k
−
H
T
+
R
)
−
1
P^-_k=AP_{k-1}A^T+Q\\ K_k = P^-_kH^{T}(HP^-_kH^{T} + R)^{-1}
Pk−=APk−1AT+QKk=Pk−HT(HPk−HT+R)−1
将
P
k
−
P^-_k
Pk−代入
K
k
K_k
Kk中
K
k
=
(
A
P
k
−
1
A
T
+
Q
)
H
T
H
(
A
P
k
−
1
A
T
+
Q
)
H
T
+
R
=
A
P
k
−
1
A
T
H
T
+
Q
H
T
H
A
P
k
−
1
A
T
H
T
+
Q
H
T
+
R
\begin{aligned} K_k &= \frac{(AP_{k-1}A^T+Q)H^T}{H(AP_{k-1}A^T+Q)H^T+R}\\ &=\frac{AP_{k-1}A^TH^T+QH^T}{HAP_{k-1}A^TH^T+QH^T+R} \end{aligned}
Kk=H(APk−1AT+Q)HT+R(APk−1AT+Q)HT=HAPk−1ATHT+QHT+RAPk−1ATHT+QHT
所以我们知道,
K
a
l
m
a
n
Kalman
Kalman
G
a
i
n
Gain
Gain的调节和过程噪声
Q
Q
Q以及观测噪声
R
R
R都有关系
再集合最优估计(后验估计)公式:
x
^
k
=
x
^
k
−
+
K
k
(
z
k
−
H
x
^
k
−
)
\hat{x}_k=\hat{x}^-_k+K_k(z_k-H\hat{x}^-_k)
x^k=x^k−+Kk(zk−Hx^k−)
1)当我们要更信任观测值
z
k
z_k
zk时,那么我们需要将
K
k
K_k
Kk增大,所以可以将观测噪声
R
R
R调小
2)当我们要更信任估计值
x
^
k
\hat{x}_k
x^k时,那么我们需要将
K
k
K_k
Kk调小,所以可以将
R
R
R调大,也可以将
Q
Q
Q调大
关于 x ^ 0 \hat{x}_0 x^0的取值和 P 0 P_0 P0的取值,我们习惯将 x ^ 0 = 0 , P 0 = 1 \hat{x}_0=0,P_0=1 x^0=0,P0=1
05 实际应用举例-RoboMaster预测跟踪
5.1 建立卡尔曼滤波器模型
我们将我们能获取到的状态向量定义为 x = [ θ p i t c h θ y a w ω p i t c h ω y a w ] x=\begin{bmatrix} \theta_{pitch} \\ \theta_{yaw} \\ \omega_{pitch} \\ \omega_{yaw} \end{bmatrix} x=⎣⎢⎢⎡θpitchθyawωpitchωyaw⎦⎥⎥⎤,控制向量 u = [ a p i t c h a y a w ] u=\begin{bmatrix} a_{pitch} \\ a_{yaw} \end{bmatrix} u=[apitchayaw],观测向量定义为 z = [ θ p i t c h θ y a w ω p i t c h ω y a w ] z=\begin{bmatrix} \theta_{pitch} \\ \theta_{yaw} \\ \omega_{pitch} \\ \omega_{yaw} \end{bmatrix} z=⎣⎢⎢⎡θpitchθyawωpitchωyaw⎦⎥⎥⎤。
1)状态预测:
根据变速运动模型运动方程:
θ
k
=
θ
k
−
1
+
ω
Δ
t
+
a
Δ
t
2
2
\theta_k=\theta_{k-1}+\omega \Delta t+\frac{a\Delta t^2}{2}
θk=θk−1+ωΔt+2aΔt2,得时间更新方程,角度的先验估计值:
x
^
k
−
=
A
x
^
k
−
1
+
B
u
k
−
1
=
[
1
0
Δ
t
0
0
1
0
Δ
t
0
0
1
0
0
0
0
1
]
x
^
k
−
1
+
[
Δ
t
2
2
0
0
Δ
t
2
2
Δ
t
0
0
Δ
t
]
u
k
−
1
\begin{aligned} \hat{x}^-_k &= A\hat{x}_{k-1}+Bu_{k-1}\\ &=\begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}\hat{x}_{k-1}+\begin{bmatrix} \frac{\Delta t^2}{2} & 0 \\ 0 & \frac{\Delta t^2}{2} \\ \Delta t & 0 \\ 0 & \Delta t \end{bmatrix}u_{k-1} \end{aligned}
x^k−=Ax^k−1+Buk−1=⎣⎢⎢⎡10000100Δt0100Δt01⎦⎥⎥⎤x^k−1+⎣⎢⎢⎡2Δt20Δt002Δt20Δt⎦⎥⎥⎤uk−1
A
A
A为状态转移矩阵(state transition matrix),
B
B
B为控制矩阵(control matrix)
2)状态协方差矩阵(state covariance matrix)预测:
P
k
−
=
A
P
k
−
1
A
T
+
Q
P^-_k = AP_{k-1}A^T+Q
Pk−=APk−1AT+Q
Q
Q
Q为过程噪声(process covariance matrix)的协方差矩阵,即
c
o
v
(
ω
k
,
ω
k
)
=
E
(
ω
k
ω
k
T
)
cov(\omega_k, \omega_k)=E(\omega_k\omega^T_k)
cov(ωk,ωk)=E(ωkωkT)
3)更新卡尔曼增益
K
k
=
A
P
k
−
1
A
T
H
T
+
Q
H
T
H
A
P
k
−
1
A
T
H
T
+
Q
H
T
+
R
K_k = \frac{AP_{k-1}A^TH^T+QH^T}{HAP_{k-1}A^TH^T+QH^T+R}
Kk=HAPk−1ATHT+QHT+RAPk−1ATHT+QHT
这是卡尔曼滤波器中最重要的一个值----卡尔曼增益(Kalman Gain)。
R
R
R为测量噪声的协方差矩阵(measurement covariance matrix),表示测量值与真实值之间的差距。
4)状态更新:
x
^
k
=
x
^
k
−
+
K
k
(
z
k
−
H
x
^
k
−
)
\hat{x}_k = \hat{x}^-_k + K_k(z_k - H\hat{x}^-_k)
x^k=x^k−+Kk(zk−Hx^k−)
完成了当前状态向量
x
x
x的更新,不仅考虑了上一时刻的预测值,也考虑了测量值,和整个系统的噪声。
5)协方差更新:
P
k
=
(
I
−
K
k
H
)
P
k
−
P_k = (I - K_kH)P^-_k
Pk=(I−KkH)Pk−
根据卡尔曼增益,更新系统的协方差
P
k
P_k
Pk用于下一个周期中协方差(
P
k
+
1
−
P^-_{k+1}
Pk+1−)预测的求解。
5.2 核心思想
核心思想:通过陀螺仪当前角度与装甲板相对云台角度的融合,得到装甲板在陀螺仪零轴上基坐标系上的绝对角度,将该角度输入至卡尔曼滤波器作为初始状态向量的角度值,之后主要作为观测向量的角度值。利用时间信息,经过新旧观测值相减,可以得到速度和加速度,输入到目标模型,目标运动模型选用变加速度模型,经过卡尔曼滤波器的更新预测,输出预测角度,实现对装甲板目标的预测跟踪。
5.3 算法流程
算法流程:
- 1、 识别到装甲板,利用 PnPSolver 算法解算得到目标在相机坐标系的坐标,再转换到机器人云台坐标系上,利用该坐标得到目标相对机器人云台中心的 Pitch 和 Yaw 角度;
- 2、 从串口读取接收电控发送到上位机来的 Pitch 和 Yaw 角(陀螺仪);
- 3、 验证电控角度是否正确(排除 NAN 值);
- 4、 陀螺仪角度与目标相对云台中心的角度融合,得到目标在陀螺仪零轴上基坐标系上的绝对角度;
- 5、 输入卡尔曼滤波器进行预测:
- 5.1、第一次进入:初始化过程噪声矩阵 Q、测量噪声矩阵 R、测量矩阵 H、误差协方差矩阵 P;更新计时器 t1、状态向量 x;
- 5.2、其余时候:更新计时器 t2、计算 t1 与 t2 的间隔时间 diff_time,更新计时器 t1。更新观测向量 z(里面包括角度和速度)、状态转移矩阵 A、控制矩阵 B 和控制向量 u。
- 5.3、每一次最终会输出后验估计状态向量 x,其中的角度信息即作为预测角度;
- 6、重力补偿;
- 7、写入 Pitch 和 Yaw 角度值至串口,发送至下位机。
5.4 自瞄预测流程图
参考文献/文章
[1]卡尔曼滤波算法详细推导
[2]A New Approach to Linear Filtering and Prediction Problems
[3]卡尔曼滤波详解
[4]卡尔曼增益超详细数学推导