⭐️卡尔曼滤波是很重要的一个算法
🌞下面我结合书上和b站视频(来源注明文末~)的内容整理了一下笔记 ~~ 主要是对卡尔曼滤波的几个公式的推导过程
卡尔曼滤波
@[toc]blog
1.滤波模型
卡尔曼滤波所要解决的问题是如何对一个离散时间线性系统的状态进行最优估算,而这一小节将先从这类线性系统的卡尔曼滤波模型谈起。
对于计算量 有数学建模等的误差
对于测量量 有测量工具的误差 人为误差等等
也就是说卡尔曼滤波是要寻找一个值平衡计算出来的计算量与测量出来的测量量
⭐️也就是一个数据融合的过程 那么这个值如何寻找，就是接下去的内容
状态方程
考虑一个有多个输入量(又称控制量)和多个输出量(又称观测量)的离散时间线性动态系统，其控制过程一般可用以下的线性差分方程式来表示:
x
(
t
k
)
=
A
(
t
k
,
t
k
−
1
)
x
(
t
k
−
1
)
+
B
(
t
k
−
1
)
u
(
t
k
−
1
)
+
w
(
t
k
−
1
)
(
6.9
)
x(t_k)=A(t_k,t_{k-1})x(t_{k-1})+B(t_{k-1})u(t_{k-1})+w(t_{k-1})\quad(6.9)
x(tk)=A(tk,tk−1)x(tk−1)+B(tk−1)u(tk−1)+w(tk−1)(6.9)
上式称为状态方程
式中tk 代表第k个测量时刻或第k测量历元所对应的时间，u 代表系统的输入向量，x 代表系统的状态向量,w 代表过程噪声向量， A ( t k , t k − 1 ) A(t_k,t_{k-1}) A(tk,tk−1)代表从tk-1到tk时刻的状态转移矩阵， B ( t k − 1 ) B(t_{k-1}) B(tk−1)代表在tk-1时刻系统输入量与系统状态之间的关系矩阵。
🐋也就是
系统在k历元的状态量=从k-1到k时的状态转移矩阵系统在k-1历元的状态量 + k-1时刻系统输入量和系统状态之间的关系矩阵* * k-1历元时的系统的输入量 + 噪声量
系统的输入量u是个可选项，即有些系统可以没有输入量，比如用户GPS 接收机这个定位系统通常视为没有任何输入。
状态转移矩阵 A ( t k , t k − 1 ) A(t_k,t_{k-1}) A(tk,tk−1)与输入关系矩阵 B ( t k − 1 ) B(t_{k-1}) B(tk−1)在不同时刻可以是不同的，但为了简化公式表达，以后我们认为它们的值固定不变。这样，式(6.9)可简写成
☘️6.9是长这样滴
x ( t k ) = A ( t k , t k − 1 ) x ( t k − 1 ) + B ( t k − 1 ) u ( t k − 1 ) + w ( t k − 1 ) ( 6.9 ) x(t_k)=A(t_k,t_{k-1})x(t_{k-1})+B(t_{k-1})u(t_{k-1})+w(t_{k-1})\quad(6.9) x(tk)=A(tk,tk−1)x(tk−1)+B(tk−1)u(tk−1)+w(tk−1)(6.9)
x
k
=
A
x
k
−
1
+
B
u
k
−
1
+
w
k
−
1
(
6.10
)
x_{k}=Ax_{k-1}+Bu_{k-1}+w_{k-1}\quad(6.10)
xk=Axk−1+Buk−1+wk−1(6.10)
式中，x 代表x(tk)，而其余各个参量的含义与此相类似。式(6.10)称为卡尔曼滤波的状态方程，它描述了系统状态如何随时间变化，是系统的动态模型，而图6.1的左半部分等价地描述了这一系统的控制过程。
由多个状态变量组成的状态向量xk,全面描述了系统在当前时刻的运行状况，它们的值通常是未知的，但一般却是我们所想要了解、掌握的。各个状态变量必须具有可观性，即它们的值能直接或间接地反映在对系统的观测量之中 ,而我们应用卡尔曼滤波器的目的正是为了达到从系统观测量yk来估算系统状态xk。
测量方程
卡尔曼滤波假定系统的状态向量xk与观测向量yk存在以下线性关系:
y
k
=
C
x
k
+
ν
k
(
6.11
)
y_{k}= Cx_{k}+\nu_{k}\quad (6.11)
yk=Cxk+νk(6.11)
上式为测量方程
系统观测量=观测量与系统状态之间的关系矩阵 * 系统状态量 +噪声量
其中，C 代表观测量与系统状态之间的关系矩阵，v 代表测量噪声向量。式(6.11)称为卡尔曼滤波的测量方程 ，它描述了当前的系统测量值与系统状态之间的关系，而图6.1的右半部分也等价地描述了这一系统的测量过程。尽管卡尔曼滤波允许测量关系矩阵的值随时间而变化，但是为了简化起见，我们在上式中将它视为一个常系数阵C。
卡尔曼滤波用上述的一个状态方程式(6.10)和一个测量方程式(6.11)来完整描述一个线性动态系统，它们是该线性动态系统的数学模型。 对于一个有L个输入变量、M个观测变量以及N个状态变量的系统，卡尔曼滤波器涉及以下一些系统参变量:
(1) x k x_{k} xk :一个由N个状态变量 ( x 1 , k , x 2 , k , ⋯ , x N , k ) (x_{1,k},x_{2,k},\cdots,x_{N,k}) (x1,k,x2,k,⋯,xN,k)所组成的N×1的状态向量。
(2) u k u_{k} uk :一个由L个输入变量 ( u 1 , k , u 2 , k , ⋯ , u L , k ) (u_{1,k},u_{2,k},\cdots,u_{L,k}) (u1,k,u2,k,⋯,uL,k)所组成的L×1的输入向量。
(3) y k y_{k} yk:一个由M个观测变量 ( y 1 , k , y 2 , k , ⋯ , y M , k ) (y_{1,k},y_{2,k},\cdots,y_{M,k}) (y1,k,y2,k,⋯,yM,k)所组成的M×1的观测向量。
(4) w k w_{k} wk :一个由N个过程噪声 ( w 1 , k , w 2 , k , ⋯ , w N , k ) (w_{1,k},w_{2,k},\cdots,w_{N,k}) (w1,k,w2,k,⋯,wN,k)所组成的Nx1的过程噪声向量。
(5) v k v_{k} vk :一个由M个测量噪声 ( v 1 , k , v 2 , k , ⋯ , v M , k ) (v_{1,k},v_{2,k},\cdots,v_{M,k}) (v1,k,v2,k,⋯,vM,k)所组成的M×1的测量噪声向量。
(6) A A A :一个NxN的状态转移矩阵。
(7) B B B:一个N×L的输入关系矩阵。
(8) C C C:一个M×N的测量关系矩阵。
🌿这个图再放一下，唤醒一下记忆~
关于噪声变量
向量
w
k
w_{k}
wk 中的各个过程噪声变量代表系统输入量
u
k
u_{k}
uk所包含的以及由系统内部所产生的随机噪声误差。卡尔曼滤波假定向量
w
k
\boldsymbol{w_{k}}
wk中的每个过程噪声变量为一个均值为零的正态白噪声，即
E
(
w
k
)
=
0
(
6.12
)
C
o
v
(
w
k
)
=
E
(
w
k
w
k
T
)
=
Q
(
6.13
)
\begin{aligned}E(\boldsymbol{w}_k)&=\boldsymbol{0}&(6.12)\\\mathrm{Cov}(\boldsymbol{w}_k)&=E(\boldsymbol{w}_k\boldsymbol{w}_k^\mathrm{T})=\boldsymbol{Q}&(6.13)\end{aligned}
E(wk)Cov(wk)=0=E(wkwkT)=Q(6.12)(6.13)
🌻 这里关于协方差的知识做一个补充
具体可以看这个blog：
https://blog.csdn.net/shijin1996/article/details/105581052/
C o v ( X , Y ) = E [ ( X − μ x ) ( Y − μ y ) ] Cov(X,Y)=E[(X-\mu_x)(Y-\mu_y)] Cov(X,Y)=E[(X−μx)(Y−μy)]
该公式可以有如下理解：如果有X,Y两个变量，每个时刻的“X值与其均值之差”乘以“Y值与其均值之差”得到一个乘积，再对这每时刻的乘积求和并求出均值.
1.协方差可以反应两个变量的协同关系， 变化趋势是否一致。同向还是方向变化。
2.X变大，同时Y也变大，说明两个变量是同向变化的，这时协方差就是正的。
3.X变大，同时Y变小，说明两个变量是反向变化的，这时协方差就是负的。
4.从数值来看，协方差的数值越大，两个变量同向程度也就越大。反之亦然。在协方差中，当只有一个变量（如x）时，cov(x)表示该变量与自身的协方差。协方差衡量两个随机变量之间的线性关系强度和方向。
🌱这里是和自身的协方差
与自身的协方差通常称为变量的方差。方差衡量一个随机变量在其期望值附近的变动范围，是随机变量离其期望值的平均偏离程度的平方。
具体而言，与自身的协方差表示一个变量自己的变化程度。如果方差较大，则表明随机变量的取值相对较分散或离散；而方差较小，则表明随机变量的取值相对集中或稳定。
也就是说
w
k
w_{k}
wk，的概率分布呈N(0,Q)。 过程噪声向量
w
k
w_{k}
wk的协方差矩阵Q为一个N×N的对称矩阵，但它并不一定是个对角阵。
向量
v
k
v_{k}
vk中的各个测量噪声变量代表系统观测量
y
k
y_{k}
yk所包含的随机测量误差与噪声。卡尔曼滤波假定每个测量噪声变量也是一个均值为零的正态白噪声，即
E
(
ν
k
)
=
0
(
6.14
)
C
o
v
(
ν
k
)
=
E
(
ν
k
ν
k
T
)
=
R
(
6.15
)
\begin{aligned}E(\nu_k)&=0&(6.14)\\\mathrm{Cov}(\nu_k)&=E(\nu_k\nu_k^\mathrm{T})=R&(6.15)\end{aligned}
E(νk)Cov(νk)=0=E(νkνkT)=R(6.14)(6.15)
也就是说，
v
k
∼
N
(
0
,
R
)
v_k\sim N(0,{R})
vk∼N(0,R)。 同样，测量噪声向量
v
k
v_k
vk的协方差矩阵R为一个M×M 的对称矩阵，但它并不一定是个对角阵。
尽管噪声向量
w
k
w_k
wk和
v
k
v_k
vk的值是未知的,但它们的协方差矩阵Q和R对卡尔曼滤波器来说是已知的。尽管协方差矩阵Q和R的值一般会随着时间的不同而变化，但是为了简化公式表达，我们这里认为它们均是一个常系数矩阵。此外，卡尔曼滤波假定过程噪声向量
w
k
w_k
wk中的各分量与测量噪声向量
v
k
v_k
vk中的各分量互不相关，即
E
(
w
i
v
j
T
)
=
θ
E(w_{i}v_{j}^{\mathrm{T}})=\theta
E(wivjT)=θ
其中，整数
i
i
i 和
j
j
j 均为任一有效历元。
2.滤波算法
卡尔曼滤波用一套数学递推公式对系统状态进行最优估计,使系统状态的估计值有最小均方误差（MMSE )。 假设 x ^ k − 1 \hat{x}_{k-1} x^k−1 代表第k-1个历元时卡尔曼滤波器对系统状态 x k − 1 {x}_{k-1} xk−1的最优估计值，那么在同时给出输入量 u k − 1 {u}_{k-1} uk−1和观测量 y k {y}_{k} yk的条件下，我们在这一小节来推导第k个历元时卡尔曼滤波器对系统状态 x k {x}_{k} xk的最优估计值 x ^ k \hat{x}_{k} x^k。与6.2节中的表达方式相同，上标“^”依然代表估计值。
从
x
^
k
−
1
\hat{x}_{k-1}
x^k−1和
u
^
k
−
1
\hat{u}_{k-1}
u^k−1出发，卡尔曼滤波利用状态方程式(6.10)来预测第k历元时的状态
x
k
{x}_{k}
xk ，即
x
^
k
−
=
A
x
^
k
−
1
+
B
u
k
−
1
(
6.17
)
\hat{x}_{k}^{-}=A\hat{x}_{k-1}+Bu_{k-1}\quad(6.17)
x^k−=Ax^k−1+Buk−1(6.17)
6.10的式子 卡尔曼滤波的状态方程
x k = A x k − 1 + B u k − 1 + w k − 1 ( 6.10 ) x_{k}=Ax_{k-1}+Bu_{k-1}+w_{k-1}\quad(6.10) xk=Axk−1+Buk−1+wk−1(6.10)
因为估计值
x
^
k
−
\hat{x}_{k}^{-}
x^k−还没有得到测量值
y
k
y_k
yk的验证，所以
x
^
k
−
\boldsymbol{\hat{x}_{k}^{-}}
x^k−通常被称为先验估计值，并用右上标“-”代表先验。先验估计误差
e
^
k
−
\boldsymbol{\hat{e}_{k}^{-}}
e^k−定义为系统状态的真实值
x
k
x_k
xk与其先验估计值
x
^
k
−
\hat{x}_{k}^{-}
x^k−之间的差异，即
e
k
−
=
x
k
−
x
^
k
−
(
6.18
)
e_{k}^{-}=x_{k}-\hat{x}_{k}^{-}\quad(6.18)
ek−=xk−x^k−(6.18)
先验估计误差
e
^
k
−
\hat{e}_{k}^{-}
e^k−的协方差阵
P
^
k
−
\hat{P}_{k}^{-}
P^k−称为状态均方误差阵（或称误差协方差阵)，它可直接根据协方差的定义而表达成
P
k
−
=
E
(
e
k
−
e
k
−
T
)
(
6.19
)
P_{k}^{-}=E(e_{k}^{-}e_{k}^{-\mathrm{T}})\quad(6.19)
Pk−=E(ek−ek−T)(6.19)
有了对当前状态
x
k
x_k
xk的先验估计值
x
^
k
−
\hat{x}_{k}^{-}
x^k−，我们就可根据测量方程式(6.11)而认为第k历元的观测量值应该等于
C
x
^
k
−
C\hat{x}_{k}^{-}
Cx^k− 再加上未知的测量噪声
v
k
v_k
vk。观测量的实际值
y
k
y_k
yk与其预测值
C
x
^
k
−
C\hat{x}_{k}^{-}
Cx^k−之间的差异，即
r
k
=
y
k
−
C
x
^
k
−
(
6.20
)
r_{k}=y_{k}-C\hat{x}_{k}^{-}\quad(6.20)
rk=yk−Cx^k−(6.20)
回顾一下6.11
y k = C x k + ν k ( 6.11 ) y_{k}= Cx_{k}+\nu_{k}\quad (6.11) yk=Cxk+νk(6.11)
r
k
r_{k}
rk称为观测量的残余。接着，**卡尔曼滤波将先验估计值
x
^
k
−
{\hat{x}_{k}^{-}}
x^k−与观测量残余
r
k
r_{k}
rk的线性组合作为对状态
x
k
x_{k}
xk的最优估计值
x
^
k
\hat{x}_{k}
x^k，**即
x
^
k
=
x
^
k
−
+
K
k
r
k
=
x
^
k
−
+
K
k
(
y
k
−
C
x
^
k
−
)
(
6.21
)
\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}r_{k}=\hat{x}_{k}^{-}+K_{k}(y_{k}-C\hat{x}_{k}^{-})\quad(6.21)
x^k=x^k−+Kkrk=x^k−+Kk(yk−Cx^k−)(6.21)
其中，系数矩阵
K
k
K_{k}
Kk称为卡尔曼滤波增益。因为实际观测量
y
k
y_{k}
yk已经核对、校正了估计值
x
^
k
\hat{x}_{k}
x^k，所以
x
^
k
\hat{x}_{k}
x^k通常称为后验估计值。
类似于式(6.18)和式(6.19)，我们可定义后验估计值
x
^
k
\hat{x}_{k}
x^k的误差
e
k
{e}_{k}
ek为
e
k
=
x
k
−
x
^
k
(
6.22
)
e_{k}=x_{k}-\hat{x}_{k}\quad(6.22)
ek=xk−x^k(6.22)
定义后验估计误差
e
k
{e}_{k}
ek的均方误差阵
P
k
{P}_{k}
Pk为
P
k
=
E
(
e
k
e
k
T
)
(
6.23
)
P_{k}=E(e_{k}e_{k}^{\mathrm{T}})\quad(6.23)
Pk=E(ekekT)(6.23)
上述后验估计均方误差阵Pk中的各个对角线元素分别对应于各个状态变量估计值的均方误差。下面要做的是推导出增益矩阵Kk的最优值，从而使得均方误差阵Рk的对角线元素之和最小，那么由此得到的卡尔曼滤波状态估计值
x
^
k
\hat{x}_{k}
x^k就有最小均方误差，相应地卡尔曼滤波器在此意义上也就是一种最优的滤波器。
K P 的推导
为了推导
K
k
K_k
Kk ，我们得先对
e
k
e_k
ek与
P
k
P_k
Pk进行一些整理。将式(6.11)、式(6.18)和式(6.21)代入式(6.22)，经整理后
y
k
=
C
x
k
+
ν
k
(
6.11
)
e
k
−
=
x
k
−
x
^
k
−
(
6.18
)
x
^
k
=
x
^
k
−
+
K
k
r
k
=
x
^
k
−
+
K
k
(
y
k
−
C
x
^
k
−
)
(
6.21
)
e
k
=
x
k
−
x
^
k
(
6.22
)
y_{k}= Cx_{k}+\nu_{k}\quad (6.11) \\ e_{k}^{-}=x_{k}-\hat{x}_{k}^{-}\quad(6.18)\\ \hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}r_{k}=\hat{x}_{k}^{-}+K_{k}(y_{k}-C\hat{x}_{k}^{-})\quad(6.21)\\ e_{k}=x_{k}-\hat{x}_{k}\quad(6.22)
yk=Cxk+νk(6.11)ek−=xk−x^k−(6.18)x^k=x^k−+Kkrk=x^k−+Kk(yk−Cx^k−)(6.21)ek=xk−x^k(6.22)
得
e
k
=
(
I
−
K
k
C
)
e
k
−
−
K
k
ν
k
(
6.24
)
e_{k}=(I-K_{k}C)e_{k}^{-}-K_{k}\nu_{k}\quad(6.24)
ek=(I−KkC)ek−−Kkνk(6.24)
注 其中 I I I是指的是单位矩阵 不同课本也用E表示单位矩阵
具体推导的过程
再将（6.15） （6.19） （6.24）代入（6.23）
E
(
ν
k
)
=
0
(
6.14
)
C
o
v
(
ν
k
)
=
E
(
ν
k
ν
k
T
)
=
R
(
6.15
)
P
k
−
=
E
(
e
k
−
e
k
−
T
)
(
6.19
)
P
k
=
E
(
e
k
e
k
T
)
(
6.23
)
\begin{aligned}E(\nu_k)&=0&(6.14)\\\mathrm{Cov}(\nu_k)&=E(\nu_k\nu_k^\mathrm{T})=R&(6.15)\end{aligned} \\ P_{k}^{-}=E(e_{k}^{-}e_{k}^{-\mathrm{T}})\quad(6.19)\\P_{k}=E(e_{k}e_{k}^{\mathrm{T}})\quad(6.23)
E(νk)Cov(νk)=0=E(νkνkT)=R(6.14)(6.15)Pk−=E(ek−ek−T)(6.19)Pk=E(ekekT)(6.23)
得
P
k
=
P
k
−
−
K
k
C
P
k
−
−
(
K
k
C
P
k
−
)
T
+
K
k
(
C
P
k
−
C
T
+
R
)
K
k
T
(
6.25
)
\boldsymbol{P_{k}}=\boldsymbol{P}_{k}^{-}-\boldsymbol{K}_{k}\boldsymbol{C}\boldsymbol{P}_{k}^{-}-\left(\boldsymbol{K}_{k}\boldsymbol{C}\boldsymbol{P}_{k}^{-}\right)^{\mathrm{T}}+\boldsymbol{K}_{k}\left(\boldsymbol{C}\boldsymbol{P}_{k}^{-}\boldsymbol{C}^{\mathrm{T}}+\boldsymbol{R}\right)\boldsymbol{K}_{k}^{\mathrm{T}}\quad(6.25)
Pk=Pk−−KkCPk−−(KkCPk−)T+Kk(CPk−CT+R)KkT(6.25)
关于计算的过程 我也放了一个参考的
P k = P k − − K k C P k − − ( K k C P k − ) T + K k ( C P k − C T + R ) K k T ( 6.25 ) \boldsymbol{P_{k}}=\boldsymbol{P}_{k}^{-}-\boldsymbol{K}_{k}\boldsymbol{C}\boldsymbol{P}_{k}^{-}-\left(\boldsymbol{K}_{k}\boldsymbol{C}\boldsymbol{P}_{k}^{-}\right)^{\mathrm{T}}+\boldsymbol{K}_{k}\left(\boldsymbol{C}\boldsymbol{P}_{k}^{-}\boldsymbol{C}^{\mathrm{T}}+\boldsymbol{R}\right)\boldsymbol{K}_{k}^{\mathrm{T}}\quad(6.25) Pk=Pk−−KkCPk−−(KkCPk−)T+Kk(CPk−CT+R)KkT(6.25)
在对式(6.25)的推导过程中，我们需要利用到先验估计误差 e k − e_k^- ek−与当前历元的测量噪声 v k v_k vk之间互不相关的假设。
为了对上式中Pk的对角线元素之和进行求导，我们得借助以下两个求导公式
d ( t r ( F G ) ) d F = G T (6.26) d ( t r ( F H F T ) ) d F = 2 F H ( 6.27 ) \begin{gathered} \frac{\mathrm{d}\left(\mathrm{tr}(\boldsymbol{FG})\right)}{\mathrm{d}\boldsymbol{F}}=\boldsymbol{G}^\mathrm{T} \text{(6.26)} \\ \frac{\mathrm{d}\left(\mathrm{tr}(\boldsymbol{FHF}^\mathrm{~T})\right)}{\mathrm{dF}}=2\boldsymbol{FH} (6.27) \end{gathered} dFd(tr(FG))=GT(6.26)dFd(tr(FHF T))=2FH(6.27)
其中，F,G和H可为任意矩阵，但是式(6.26)中的乘积FG必须为方阵，而式(6.27)中的H必须为对称阵。运算符“tr”代表矩阵的求迹算子，即计算矩阵的对角线元素之和。
具体推导
式(6.25)表明，Pk或者说tr(Pk)是一个关于Kk的二次型函数。因为
(
(
C
P
k
−
C
T
+
R
)
((\boldsymbol{C}\boldsymbol{P}_k^-\boldsymbol{C}^\mathrm{T}+\boldsymbol{R})
((CPk−CT+R)正定，所以关于Kk的二次型函数 tr(Pk)存在最小值。将tr(Pk)对Kk求导，并使其导数值等于零，我们就可得到使tr(Pk)值最小的Kk的解，即
d
(
tr
(
P
k
)
)
d
K
k
=
−
2
(
C
P
k
−
)
T
+
2
K
k
(
C
P
k
−
C
T
+
R
)
(
6.28
)
\frac{\mathrm{d}\left(\operatorname{tr}(\boldsymbol{P}_k)\right)}{\mathrm{d}\boldsymbol{K}_k}=-2\left(\boldsymbol{C}\boldsymbol{P}_k^-\right)^\mathrm{T}+2\boldsymbol{K}_k\left(\boldsymbol{C}\boldsymbol{P}_k^-\boldsymbol{C}^\mathrm{T}+\boldsymbol{R}\right)\boldsymbol\quad(6.28)
dKkd(tr(Pk))=−2(CPk−)T+2Kk(CPk−CT+R)(6.28)
K k = P k − C T ( C P k − C T + R ) − 1 ( 6.29 ) K_k=P_k^-C^\mathrm{T}\left(CP_k^-C^\mathrm{T}+R\right)^{-1}\quad(6.29) Kk=Pk−CT(CPk−CT+R)−1(6.29)
再将式(6.29)中的Kk代回到式(6.25)，得
P k = ( I − K k C ) P k − ( 6.30 ) P_k=(I-K_kC)P_k^-\quad(6.30) Pk=(I−KkC)Pk−(6.30)
x ^ k = x ^ k − + K k r k = x ^ k − + K k ( y k − C x ^ k − ) ( 6.21 ) \hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}r_{k}=\hat{x}_{k}^{-}+K_{k}(y_{k}-C\hat{x}_{k}^{-})\quad(6.21) x^k=x^k−+Kkrk=x^k−+Kk(yk−Cx^k−)(6.21)
回到式（6.21）结合式（6.30）和（6.29），我们可以得到：
当测量误差R(测量噪声）很小的时候，得到的就是测量结果。当测量误差R很大的时候，就是我们的估计（用公式/数学模型等）计算的结果。
3.总结
在此，我们将卡尔曼滤波的递推算法总结一下。如图6.2所示，卡尔曼滤波算法可分为预测和校正两个过程。
🌺(1)预测 :预测过程又叫时间更新过程，它在上一(即第k-1)个历元状态估计值的基础上，利用系统的状态方程来预测当前这一（即第k )个历元的状态值。这一过程涉及以下两个公式:
x
^
k
−
=
A
x
^
k
−
1
+
B
u
k
−
1
(
6.31
)
P
k
−
=
A
P
k
−
1
A
T
+
Q
(
6.32
)
\begin{aligned}\hat{\boldsymbol{x}}_k^-&=A\hat{\boldsymbol{x}}_{k-1}+B\boldsymbol{u}_{k-1}&(6.31)\\\boldsymbol{P}_k^-&=A\boldsymbol{P}_{k-1}A^\mathrm{T}+\boldsymbol{Q}&(6.32)\end{aligned}
x^k−Pk−=Ax^k−1+Buk−1=APk−1AT+Q(6.31)(6.32)
对于6.32 在这里推导一下 可能一开始会很懵
x k = A x k − 1 + B u k − 1 + w k − 1 ( 6.10 ) E ( w k ) = 0 ( 6.12 ) C o v ( w k ) = E ( w k w k T ) = Q ( 6.13 ) P k − = E ( e k − e k − T ) ( 6.19 ) e k − = x k − x ^ k − ( 6.18 ) x ^ k − = A x ^ k − 1 + B u k − 1 ( 6.17 ) x_{k}=Ax_{k-1}+Bu_{k-1}+w_{k-1}\quad(6.10)\\ \begin{aligned}E(\boldsymbol{w}_k)&=\boldsymbol{0}&(6.12)\\ {Cov}(\boldsymbol{w}_k)&=E(\boldsymbol{w}_k\boldsymbol{w}_k^\mathrm{T})=\boldsymbol{Q}&(6.13)\end{aligned} \\ P_{k}^{-}=E(e_{k}^{-}e_{k}^{-\mathrm{T}})\quad(6.19) \\e_{k}^{-}=x_{k}-\hat{x}_{k}^{-}\quad(6.18) \\\hat{x}_{k}^{-}=A\hat{x}_{k-1}+Bu_{k-1}\quad(6.17) xk=Axk−1+Buk−1+wk−1(6.10)E(wk)Cov(wk)=0=E(wkwkT)=Q(6.12)(6.13)Pk−=E(ek−ek−T)(6.19)ek−=xk−x^k−(6.18)x^k−=Ax^k−1+Buk−1(6.17)
其中，式(6.31)只是重复了式(6.17)，它利用状态方程来预测当前的系统状态。在卡尔曼滤波中，每一个状态估计值必须跟随一个用来衡量k此状态估计值可靠性的均方误差阵。 在预测状态时，由于过程噪声wk的值未知，因而状态先验估计值的均方误差阵计算公式(6.32)添加了过程噪声的协方差Q以降低状态先验估计值的可靠性和保持均方误差阵的正确性。在完成这一预测过程后，系统的状态估计均方误差通常会变大。
(2)校正: 校正过程又叫测量更新过程，它是利用实际测量值来校正经上一步预测得到的状态先验估计值。这一过程涉及以下三个公式:
K
k
=
P
k
−
C
T
(
C
P
k
−
C
T
+
R
)
−
l
(6.33)
x
^
k
=
x
^
k
−
+
K
k
(
y
k
−
C
x
^
k
−
)
(6.34)
P
k
=
(
I
−
K
k
C
)
P
k
−
(6.35)
\begin{gathered} K_{k}=P_{k}^{-}C^{\mathrm{T}}\left(CP_{k}^{-}C^{\mathrm{T}}+R\right)^{-\mathrm{l}} \text{(6.33)} \\ \hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}\left(\boldsymbol{y}_{k}-\boldsymbol{C}\hat{\boldsymbol{x}}_{k}^{-}\right) \text{(6.34)} \\ P_k=(I-K_kC)P_k^- \text{(6.35)} \end{gathered}
Kk=Pk−CT(CPk−CT+R)−l(6.33)x^k=x^k−+Kk(yk−Cx^k−)(6.34)Pk=(I−KkC)Pk−(6.35)
其中，式(6.33)就是我们前面推导出的卡尔曼增益计算公式 (6.29)，而式(6.34)和式(6.35)对系统的状态估计值及其均方误差进行更新。
在这一过程中，由于状态估计值得到了实际测量值的验证，因而它的均方误差值变小，即可靠性增加。 卡尔曼滤波器综合、平衡了它的先验估计和实际测量这两方面的信息，使测量更新以后的状态估计值具有最小的均方误差。
⭐️ 关于初值
如图6.2所示，卡尔曼滤波器需要外界提供系统状态及其均方误差的初始估计值，并且要求状态初始估计值为当时状态真实值的均值，即
x
^
0
=
E
(
x
0
)
(
6.36
)
\hat{x}_{0}=E(x_{0})\quad(6.36)
x^0=E(x0)(6.36)
而相应的状态均方误差的初始估计值P0满足
P
0
=
E
(
(
x
0
−
x
^
0
)
(
x
0
−
x
^
0
)
T
)
(
6.37
)
P_{0}=E\Big((x_{0}-\hat{x}_{0})(x_{0}-\hat{x}_{0})^{\mathrm{T}}\Big)\quad(6.37)
P0=E((x0−x^0)(x0−x^0)T)(6.37)
卡尔曼滤波不但给出系统状态的估计值
x
^
k
\hat{x}_k
x^k，而且还同时给出这个估计值的均方误差Pk。可以证明，系统状态真实值xk的均值与方差分别等于后验估计值
x
^
k
\hat{x}_k
x^k与Pk，即
E
(
x
k
)
=
x
^
k
(
6.38
)
E
(
e
k
e
k
T
)
=
P
k
(
6.39
)
\begin{aligned}E(\boldsymbol{x}_{k})&=\hat{\boldsymbol{x}}_{k}&(6.38)\\E(\boldsymbol{e}_{k}\boldsymbol{e}_{k}^{\mathrm{T}})&=\boldsymbol{P}_{k}&(6.39)\end{aligned}
E(xk)E(ekekT)=x^k=Pk(6.38)(6.39)
其中，式(6.8)表明了卡尔曼滤波器可提供系统状态的无偏估计。代表状态估计值可靠性的均方误差Pk，其重要性并不亚于状态估计值
x
^
k
\hat{x}_k
x^k。如果状态估计的均方误差Рk过大，比如超过了一个门限值，那么我们应该对此时的滤波结果
x
^
k
\hat{x}_k
x^k保持警惕，甚至放弃利用该滤波结果。卡尔曼滤波的这种同时提供
x
^
k
\hat{x}_k
x^k和Pk的特点在各种实际应用中非常受欢迎，而这一特点是 α-β 滤波器所没有的。
我们再回过头来讨论一下卡尔曼增益的意义。在式(6.33)中，由于测量关系矩阵C和测量误差协方差矩阵R均假定为常系数阵，因而在第k历元的增益值Kk只是一个关于状态估计均方误差阵 P k − P_k^{-} Pk−的函数。假设 P k − P_k^{-} Pk−值很大，大得使R值相对于 C P k − C T CP_{k}^{-}C^{\mathrm{T}} CPk−CT而言趋向于零，那么增益Kk值就趋向于C-1，相应地式(6.34)中的状态估计值 x ^ k \hat{x}_k x^k就基本上等于 C − 1 y k C^{-1}y_{k} C−1yk。这就是说，如果先验估计值 x ^ k − \hat{x}_k^{-} x^k−相对不可靠或者测量值yk相对很准确，那么由式(6.33)计算得到的增益值会使状态估计 x ^ k \hat{x}_k x^k主倾向于信任yk而减少对 x ^ k − \hat{x}_k^{-} x^k−的依赖， 这自然相当地合情合理。
反过来，假如先验估计值 x ^ k − \hat{x}_k^{-} x^k−相对很可靠或者测量值yk相对很粗糙，也就是说， C P k − C T CP_{k}^{-}C^{\mathrm{T}} CPk−CT值相对于R而言很小，那么增益Kk的值就趋向于零，相应地状态估计值 x ^ k \hat{x}_k x^k就倾向于信任 x ^ k − \hat{x}_k^{-} x^k−而减少对yk的依赖， 这同样十分合乎逻辑。与a-β滤波器采用恒定滤波系数不同,卡尔曼滤波器能根据状态估计协方差与测量误差协方差的相对大小，自动、即时地调节增益值，以达到对系统状态的最优估计。
卡尔曼滤波器的最优运行，要求外界提供给滤波器的过程噪声方差Q和测量噪声方差R是准确的,否则卡尔曼增益和状态估计等参量不会达到它们的最优值,甚至还可能引起滤波器发散。例如，若输入给滤波器的过程噪声方差值Q严重偏小于其真实值，则卡尔曼增益值也将会严重偏小，致使测量值中所含的信息不能被及时、全部地反映在状态估计值上，最终会导致滤波器发散。对各个时刻的过程噪声和测量噪声方差值进行准确估算是卡尔曼滤波器设计、调试中重要而又困难的一步。
🌱注 本文整理自：
书还是谢钢的那本GPS原理
b站视频 大家可以看看这个！！ 讲的很好！~【【卡尔曼滤波器】https://www.bilibili.com/video/BV12D4y1S7fU/?share_source=copy_web&vd_source=8d5f94cac4ef1f7256e3572189ec255b
🌈ok,完结~ (●ˇ∀ˇ●)点个赞 点个赞~
3154
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
