卡尔曼滤波
文章目录
1贝叶斯滤波
1.1理论基础
（1）本质：通过贝叶斯公式对随机信号处理，从而减小不确定度（即方差）。
（2）随机过程：x1,…,xn为随机变量，但不独立。
主观概率（先验概率）（实验前），
引入外部观测（证据、信息），
得到相对客观的概率（后验概率）（实验后）。
（3）先验概率、后验概率、似然概率：
离散举例：温度测量
T
实
际
温
度
、
T
m
温
度
计
测
量
温
度
T实际温度、T_m温度计测量温度
T实际温度、Tm温度计测量温度
a. 先验概率分布
{
P
(
T
=
10
)
=
0.8
表
示
实
际
温
度
为
10
的
概
率
为
0.8
P
(
T
=
11
)
=
0.2
\left\{ \begin{aligned} &P(T=10) = 0.8 表示实际温度为10的概率为0.8 \\ &P(T=11)=0.2 \end{aligned} \right.
{P(T=10)=0.8表示实际温度为10的概率为0.8P(T=11)=0.2
b. 温度计测量值
T
m
T_m
Tm
c. 后验概率分布：
P
(
T
=
10
∣
T
m
=
10.3
)
=
P
(
T
m
=
10.3
∣
T
=
10
)
∗
P
(
T
=
10
)
P
(
T
m
=
10.3
)
P(T=10|T_m=10.3)=\frac{P(T_m=10.3|T=10)*P(T=10)}{P(T_m=10.3)}
P(T=10∣Tm=10.3)=P(Tm=10.3)P(Tm=10.3∣T=10)∗P(T=10)
表示在温度计显示为10.3度的条件下，实际温度为10度的概率。
其中，
P
(
T
m
=
10.3
∣
T
=
10
)
P(T_m=10.3|T=10)
P(Tm=10.3∣T=10)表示在实际温度为10度的情况下温度计测量为10.3度的概率。似然概率：观测精度/传感器精度；
P
(
T
m
=
10.3
)
P(T_m=10.3)
P(Tm=10.3)看作常数
η
\eta
η。
后验= η ∗ 似 然 ∗ 先 验 \eta*似然*先验 η∗似然∗先验，其中 η = 1 ∑ ( 似 然 ∗ 先 验 ) \eta=\frac{1}{\sum{(似然*先验)}} η=∑(似然∗先验)1。
连续：
f
X
∣
Y
(
x
∣
y
)
=
f
Y
∣
X
(
y
∣
x
)
∗
f
X
(
x
)
f
Y
(
y
)
=
η
∗
似
然
∗
后
验
f_{X|Y}(x|y)=\frac{f_{Y|X}(y|x)*f_{X}(x)}{f_{Y}(y)}=\eta*似然*后验
fX∣Y(x∣y)=fY(y)fY∣X(y∣x)∗fX(x)=η∗似然∗后验
定理：若
f
X
(
x
)
f_{X}(x)
fX(x)~
N
(
μ
1
,
δ
1
2
)
，
f
Y
∣
X
(
y
∣
x
)
N(\mu_{1}, \delta_1^2)，f_{Y|X}(y|x)
N(μ1,δ12)，fY∣X(y∣x)~
N
(
μ
2
,
δ
2
2
)
N(\mu_2, \delta_2^2)
N(μ2,δ22)，
则
f
X
∣
Y
(
x
∣
y
)
f_{X|Y}(x|y)
fX∣Y(x∣y)~
N
(
δ
2
2
δ
1
2
+
δ
2
2
∗
μ
1
+
δ
1
2
δ
1
2
+
δ
2
2
∗
μ
2
,
δ
1
2
δ
2
2
δ
1
2
+
δ
2
2
)
.
N(\frac{\delta_2^2}{\delta_1^2+\delta_2^2}*\mu_1+\frac{\delta_1^2}{\delta_1^2+\delta_2^2}*\mu_2, \frac{\delta_1^2\delta_2^2}{\delta_1^2+\delta_2^2}).
N(δ12+δ22δ22∗μ1+δ12+δ22δ12∗μ2,δ12+δ22δ12δ22). (本质：实现方差降低)。
|
|
1.2贝叶斯滤波算法
X-先验，Y-观测，
Q
k
Q_k
Qk-预测噪声，
R
k
R_k
Rk-观测噪声
（1）过程
x1,…,xn由递推得：
（2）前提假设：
a
.
{
状
态
方
程
：
X
k
=
f
(
X
k
−
1
)
+
Q
k
观
测
方
程
：
Y
k
=
h
(
X
k
)
+
R
k
a. \left\{ \begin{aligned} &状态方程：X_k=f(X_{k-1})+Q_k \\ &观测方程：Y_k=h(X_k)+R_k \end{aligned} \right.
a.{状态方程：Xk=f(Xk−1)+Qk观测方程：Yk=h(Xk)+Rk
b.
X
k
、
X
k
−
1
、
Y
k
、
Q
k
、
R
k
X_k、X_{k-1}、Y_k、Q_k、R_k
Xk、Xk−1、Yk、Qk、Rk均为随机变量；
X
0
、
Q
1
.
.
.
Q
k
、
R
1
.
.
.
R
k
X_0、Q_1...Q_k、R_1...R_k
X0、Q1...Qk、R1...Rk相互独；
X
0
X_0
X0 ~
f
0
(
x
)
，
Q
k
f_0(x)，Q_k
f0(x)，Qk ~
f
Q
k
(
x
)
，
R
k
f_{Q_k}(x)，R_k
fQk(x)，Rk ~
f
R
k
(
x
)
f_{R_k}(x)
fRk(x)。
（3）预测步、更新步：
|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
2卡尔曼滤波
（1）前提假设：
a.
f
(
X
k
−
1
)
=
F
∗
X
k
−
1
，
h
(
X
k
)
=
H
∗
X
k
f(X_{k-1})=F*X_{k-1}，h(X_k)=H*X_k
f(Xk−1)=F∗Xk−1，h(Xk)=H∗Xk，F、H为常数；
b. Q~ N(0, q), R~ N(0,r)。
（2）预测步、更新步
（3）五个公式：
{
μ
k
−
=
F
∗
μ
k
−
1
+
δ
k
−
=
F
2
∗
δ
k
−
1
+
+
q
卡
尔
曼
增
益
K
=
H
∗
δ
k
−
H
2
∗
δ
k
−
+
r
μ
k
+
=
K
∗
(
y
k
−
H
∗
μ
k
−
)
+
μ
k
−
δ
k
+
=
(
1
−
H
∗
K
)
∗
δ
k
−
\left\{ \begin{aligned} &\mu_k^-=F*\mu_{k-1}^+\\ &\delta_k^-=F^2*\delta_{k-1}^++q\\ &卡尔曼增益K=\frac{H*\delta_k^-}{H^2*\delta_k^-+r}\\ &\mu_k^+=K*(y_k-H*\mu_k^-)+\mu_k^-\\ &\delta_k^+=(1-H*K)*\delta_k^- \end{aligned} \right.
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧μk−=F∗μk−1+δk−=F2∗δk−1++q卡尔曼增益K=H2∗δk−+rH∗δk−μk+=K∗(yk−H∗μk−)+μk−δk+=(1−H∗K)∗δk−
（4）矩阵形式：
μ
k
→
μ
k
⃗
，
δ
k
→
δ
k
⃗
\mu_k\rightarrow\vec{\mu_k}，\delta_k\rightarrow\vec{\delta_k}
μk→μk
，δk→δk
，F、H均为矩阵。
{
μ
k
−
⃗
=
F
∗
μ
k
−
1
+
⃗
δ
k
−
⃗
=
F
∗
δ
k
−
1
+
⃗
∗
F
T
+
q
卡
尔
曼
增
益
K
=
H
∗
δ
k
−
⃗
H
∗
δ
k
−
⃗
∗
H
T
+
r
μ
k
+
⃗
=
K
∗
(
y
k
⃗
−
H
∗
μ
k
−
⃗
)
+
μ
k
−
⃗
δ
k
+
⃗
=
(
1
−
H
∗
K
)
∗
δ
k
−
⃗
\left\{ \begin{aligned} &\vec{\mu_k^-}=F*\vec{\mu_{k-1}^+}\\ &\vec{\delta_k^-}=F*\vec{\delta_{k-1}^+}*F^T+q\\ &卡尔曼增益K=\frac{H*\vec{\delta_k^-}}{H*\vec{\delta_k^-}*H^T+r}\\ &\vec{\mu_k^+}=K*(\vec{y_k}-H*\vec{\mu_k^-})+\vec{\mu_k^-}\\ &\vec{\delta_k^+}=(1-H*K)*\vec{\delta_k^-} \end{aligned} \right.
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧μk−
=F∗μk−1+
δk−
=F∗δk−1+
∗FT+q卡尔曼增益K=H∗δk−
∗HT+rH∗δk−
μk+
=K∗(yk
−H∗μk−
)+μk−
δk+
=(1−H∗K)∗δk−
学习来源：感谢up主：忠厚老实的老王