# 卡尔曼滤波器的前言

P

(

s

=

)

=

0.6

P

(

s

=

)

=

0.4

P(s=)=0.6P(s=)=0.4

P

(

o

=

s

=

)

=

0.8

P

(

o

=

s

=

)

=

0.2

P

(

o

=

s

=

)

=

0.1

P

(

o

=

s

=

)

=

0.9

P(o=s=)=0.8P(o=s=)=0.2P(o=s=)=0.1P(o=s=)=0.9

P

(

s

=

o

=

)

=

P

(

s

=

,

o

=

)

P

(

o

=

)

=

P

(

o

=

s

=

)

P

(

s

=

)

P

(

o

=

s

=

)

P

(

s

=

)

+

P

(

o

=

s

=

)

P

(

s

=

)

=

0.2

×

0.6

0.2

×

0.6

+

0.9

×

0.4

=

0.25

P(s=男生|o=长发)=frac{P(s=男生,o=长发)}{P(o=长发)}=frac{P(o=长发|s=男生)P(s=男生)}{P(o=长发|s=男生)P(s=男生)+P(o=长发|s=女生)P(s=女生)}=frac{0.2×0.6}{0.2×0.6+0.9×0.4}=0.25

P(s=o=)=P(o=)P(s=,o=)=P(o=s=)P(s=)+P(o=s=)P(s=)P(o=s=)P(s=)=0.2×0.6+0.9×0.40.2×0.6=0.25

P

(

s

=

o

=

)

=

P

(

s

=

,

o

=

)

P

(

o

=

)

=

P

(

o

=

s

=

)

P

(

s

=

)

P

(

o

=

s

=

)

P

(

s

=

)

+

P

(

o

=

s

=

)

P

(

s

=

)

=

0.9

×

0.4

0.2

×

0.6

+

0.9

×

0.4

=

0.75

P(s=女生|o=长发)=frac{P(s=女生,o=长发)}{P(o=长发)}=frac{P(o=长发|s=女生)P(s=女生)}{P(o=长发|s=男生)P(s=男生)+P(o=长发|s=女生)P(s=女生)}=frac{0.9×0.4}{0.2×0.6+0.9×0.4}=0.75

P(s=o=)=P(o=)P(s=,o=)=P(o=s=)P(s=)+P(o=s=)P(s=)P(o=s=)P(s=)=0.2×0.6+0.9×0.40.9×0.4=0.75

PS：根据上边这个例子，直观点的解释如下。

s

s

s为隐状态，意思是我们不能直观看到，

o

o

o是观测量，是我们我可以直观看到的。就像上面的例子一样，同学的真实性别我们观测不到，是隐状态，但是我们可以根据背影看到是否为长发，是否为长发就是观测量。

x

k

=

A

x

k

1

+

B

u

k

1

+

w

k

1

x_{k}=Ax_{k-1}+Bu_{k-1}+w_{k-1}

xk=Axk1+Buk1+wk1

x

k

x_{k}

xk是系统在

k

k

k时刻的系统状态量，

x

k

:

n

×

1

x_{k}:n×1

xk:n×1

x

k

1

x_{k-1}

xk1是系统在

k

1

k-1

k1时刻的系统状态量，

x

k

1

:

n

×

1

x_{k-1}:n×1

xk1:n×1

A

A

A是系统矩阵，

A

:

n

×

n

A:n×n

A:n×n

B

B

B是输入矩阵（也叫控制矩阵），

B

:

n

×

r

B:n×r

B:n×r

u

k

1

u_{k-1}

uk1是系统在

k

1

k-1

k1时刻的系统输入，

u

k

1

:

r

×

1

u_{k-1}:r×1

uk1:r×1

w

k

1

w_{k-1}

wk1是服从高斯分布的随机向量，相当于是系统噪声，

w

k

1

:

n

×

1

w_{k-1}:ntimes 1

wk1:n×1

w

k

1

N

p

(

0

,

Q

)

w_{k-1}sim N_{p}(0,Q)

wk1Np(0,Q)

Q

Q

Q是系统噪声服从的协方差矩阵，

Q

:

n

×

n

Q:ntimes n

Q:n×n

y

k

=

H

x

k

+

v

k

y_{k}=Hx_{k}+v_{k}

yk=Hxk+vk

x

k

x_{k}

xk是系统在

k

k

k时刻的系统状态量，

x

k

:

n

×

1

x_{k}:n×1

xk:n×1

H

H

H是系统状态到观测值（系统中的传感器数据）的转换矩阵，

H

:

m

×

n

H:mtimes n

H:m×n,

v

k

v_{k}

vk是测量噪声，

v

k

:

m

×

1

v_{k}:mtimes 1

vk:m×1

v

k

N

p

(

0

,

R

)

v_{k}sim N_{p}(0,R)

vkNp(0,R)

R

R

R是测量噪声服从的协方差矩阵，

R

:

m

×

m

R:mtimes m

R:m×m

y

k

y_{k}

yk是系统的观测值（系统中的传感器数据）。

x

k

x_{k}

xk是我们直接观测不到的，即隐状态，我们能观测到的量是传感器的数据

y

k

y_{k}

yk，现在我们要推算

k

k

k时刻系统中的真实状态值

x

k

x_{k}

xk，在推测

x

k

x_{k}

xk的同时，我们已经知道系统过往的测量值

y

1

,

y

2

,

,

y

k

y_{1},y_{2},dots,y_{k}

y1,y2,,yk，系统过往的真实状态值

x

1

,

x

2

,

,

x

k

1

x_{1},x_{2},dots,x_{k-1}

x1,x2,,xk1我们是不知道的，即使我们对

x

1

,

x

2

,

,

x

k

1

x_{1},x_{2},dots,x_{k-1}

x1,x2,,xk1做出了估计。为了更好的展示该过程的逻辑，如下图所示，

k

k

k对应观测向量

y

k

y_{k}

yk（可能系统有多个传感器，所以每一次可以得到一个观测向量）。重点来了！！！现在，需要要做的是，在已知系统过往的观测值向量

y

1

,

y

2

,

,

y

k

1

y_{1},y_{2},dots,y_{k-1}

y1,y2,,yk1和当前观测值向量

y

k

y_{k}

yk的前提下，估计当前状态

x

k

x_{k}

xk最可能取到的值，其数学表达式为

x

^

k

=

argmax

P

(

x

k

y

1

,

y

2

,

,

y

k

)

hat{x}_{k}=text{argmax}P(x_{k}|y_{1},y_{2},dots,y_{k})

x^k=argmaxP(xky1,y2,,yk)

P

(

x

k

y

1

,

y

2

,

,

y

k

)

=

1

2

π

σ

e

(

x

k

μ

)

2

2

σ

2

P(x_{k}|y_{1},y_{2},dots,y_{k})=frac{1}{sqrt{2pi}sigma}e^{frac{-(x_{k}-mu)^{2}}{2sigma^{2}}}

P(xky1,y2,,yk)=2π

σ1e2σ2(xkμ)2，那么让我们估计一下

x

k

x_{k}

xk的值，我们肯定将估计值

x

^

k

hat{x}_{k}

x^k

μ

mu

μ，因为在

x

k

=

μ

x_{k}=mu

xk=μ发生的概率最大。其实卡尔曼滤波就做了这件事，计算出

k

k

k时刻的

P

(

x

k

y

1

,

y

2

,

,

y

k

)

P(x_{k}|y_{1},y_{2},dots,y_{k})

P(xky1,y2,,yk)，这个式子是多元正态分布（为什么呢？下面我就介绍），先说一下，多元正态分布表达式的一般形式，如下所示，

f

(

x

)

=

1

(

2

π

)

p

Σ

1

2

e

1

2

(

x

μ

)

T

Σ

1

(

x

μ

)

f(x)=frac{1}{(sqrt{2pi})^{p}{|Sigma|^{frac{1}{2}}}}e^{-frac{1}{2}(x-mu)^{T}Sigma^{-1}(x-mu)}

f(x)=(2π

)pΣ211e21(xμ)TΣ1(xμ)

x

x

x是随机向量，

x

:

p

×

1

x:ptimes 1

x:p×1,

Σ

Sigma

Σ是随机向量

x

x

x的协方差矩阵，

Σ

:

p

×

p

Sigma :ptimes p

Σ:p×p

μ

mu

μ是随机向量

x

x

x的均值，

μ

:

p

×

1

mu:ptimes 1

μ:p×1

f

(

x

)

f(x)

f(x)也称为随机向量

x

x

x的概率密度函数，也记作

x

N

p

(

μ

,

Σ

)

xsim N_{p}(mu,Sigma)

xNp(μ,Σ)

# 卡尔曼滤波器的推导

x

k

=

A

x

k

1

+

B

u

k

1

+

w

k

1

x_{k}=Ax_{k-1}+Bu_{k-1}+w_{k-1}

xk=Axk1+Buk1+wk1

y

k

=

H

x

k

+

v

k

y_{k}=Hx_{k}+v_{k}

yk=Hxk+vk

t

=

1

t=1

t=1时，系统刚上电，此时系统中的观测值为

y

1

y_{1}

y1，在观测值

y

1

y_{1}

y1发生的前提下，我们假设

P

(

x

1

y

1

)

N

n

(

μ

^

1

,

Σ

^

1

)

P(x_{1}|y_{1})sim N_{n}(hat{mu}_{1},hat{Sigma}_{1})

P(x1y1)Nn(μ^1,Σ^1)

μ

^

1

hat{mu}_{1}

μ^1直接取作系统的初始状态值就行了，因为系统中是存在不确定性（噪声），如果噪声很小，我们

Σ

^

1

hat{Sigma}_{1}

Σ^1就可以方差取小一些，反之，取大一些。这个算是，我们使用卡尔曼滤波器的一个超参数，是需要我们提前做出假设的。PS：肯定有人会问，如果我们开始这个假设和系统真实情况

P

(

x

1

y

1

)

P(x_{1}|y_{1})

P(x1y1)不太符合怎么办，其实问题不大，后面再迭代过程中，这个初始假设中存在的不合理性，会慢慢弱化。后面仿真，会对这个初始假设做进一步说明。

t

=

2

t=2

t=2的时候，我们需要计算

P

(

x

2

y

1

,

y

2

)

P(x_{2}|y_{1},y_{2})

P(x2y1,y2)

t

=

k

1

t=k-1

t=k1时，

P

(

x

k

1

y

1

,

y

2

,

,

y

k

1

)

N

n

(

μ

^

k

1

,

Σ

^

k

1

)

P(x_{k-1}|y_{1},y_{2},dots,y_{k-1})sim N_{n}(hat{mu}_{k-1},hat{Sigma}_{k-1})

P(xk1y1,y2,,yk1)Nn(μ^k1,Σ^k1)，在

t

=

k

t=k

t=k时，

P

(

x

k

y

1

,

y

2

,

,

y

k

)

=

P

(

x

k

,

y

k

y

1

,

y

2

,

,

y

k

1

)

P

(

y

k

y

1

,

y

2

,

,

y

k

1

)

P(x_{k}|y_{1},y_{2},dots,y_{k})=frac{P(x_{k},y_{k}|y_{1},y_{2},dots,y_{k-1})}{P(y_{k}|y_{1},y_{2},dots,y_{k-1})}

P(xky1,y2,,yk)=P(yky1,y2,,yk1)P(xk,yky1,y2,,yk1)

P

(

x

k

1

y

1

,

y

2

,

,

y

k

1

)

N

n

(

μ

^

k

1

,

Σ

^

k

1

)

P(x_{k-1}|y_{1},y_{2},dots,y_{k-1})sim N_{n}(hat{mu}_{k-1},hat{Sigma}_{k-1})

P(xk1y1,y2,,yk1)Nn(μ^k1,Σ^k1)，且

x

k

=

A

x

k

1

+

B

u

k

1

+

w

k

1

x_{k}=Ax_{k-1}+Bu_{k-1}+w_{k-1}

xk=Axk1+Buk1+wk1，则有

P

(

x

k

y

1

,

y

2

,

,

y

k

1

)

N

n

(

A

μ

^

k

1

+

B

u

k

1

,

A

Σ

^

k

1

A

T

+

Q

)

P(x_{k}|y_{1},y_{2},dots,y_{k-1})sim N_{n}(Ahat{mu}_{k-1}+Bu_{k-1},Ahat{Sigma}_{k-1}A^{T}+Q)

P(xky1,y2,,yk1)Nn(Aμ^k1+Buk1,AΣ^k1AT+Q)

P

(

x

k

y

1

,

y

2

,

,

y

k

1

)

N

n

(

A

μ

^

k

1

+

B

u

k

1

,

A

Σ

^

k

1

A

T

+

Q

)

P(x_{k}|y_{1},y_{2},dots,y_{k-1})sim N_{n}(Ahat{mu}_{k-1}+Bu_{k-1},Ahat{Sigma}_{k-1}A^{T}+Q)

P(xky1,y2,,yk1)Nn(Aμ^k1+Buk1,AΣ^k1AT+Q)，且

y

k

=

H

x

k

+

v

k

y_{k}=Hx_{k}+v_{k}

yk=Hxk+vk，则有

P

(

y

k

y

1

,

y

2

,

,

y

k

1

)

N

n

(

H

(

A

μ

^

k

1

+

B

u

k

1

)

,

H

(

A

Σ

^

k

1

A

T

+

Q

)

H

T

+

R

)

P(y_{k}|y_{1},y_{2},dots,y_{k-1})sim N_{n}(H(Ahat{mu}_{k-1}+Bu_{k-1}),H(Ahat{Sigma}_{k-1}A^{T}+Q)H^{T}+R)

P(yky1,y2,,yk1)Nn(H(Aμ^k1+Buk1),H(AΣ^k1AT+Q)HT+R)

P

(

x

k

y

1

,

y

2

,

,

y

k

1

)

P(x_{k}|y_{1},y_{2},dots,y_{k-1})

P(xky1,y2,,yk1)

P

(

y

k

y

1

,

y

2

,

,

y

k

1

)

P(y_{k}|y_{1},y_{2},dots,y_{k-1})

P(yky1,y2,,yk1)的分布知道了，可以得到

P

(

x

k

,

y

k

y

1

,

y

2

,

,

y

k

1

)

P(x_{k},y_{k}|y_{1},y_{2},dots,y_{k-1})

P(xk,yky1,y2,,yk1)的分布，该分布仍然是多元正态分布，分布为

P

(

x

k

,

y

k

y

1

,

y

2

,

,

y

k

1

)

N

n

+

m

(

[

A

μ

^

k

1

+

B

u

k

1

H

(

A

u

^

k

1

+

B

u

k

1

)

]

,

[

A

Σ

^

k

1

A

T

+

Q

(

A

Σ

^

k

1

A

T

+

Q

)

H

T

H

(

A

Σ

^

k

1

A

T

+

Q

)

T

H

(

A

Σ

^

k

1

A

T

+

Q

)

H

T

+

R

]

)

P(x_{k},y_{k}|y_{1},y_{2},dots,y_{k-1})sim N_{n+m}( left[ begin{array}{c} Ahat{mu}_{k-1}+Bu_{k-1}\ H(Ahat{u}_{k-1}+Bu_{k-1}) end{array} right], left[ begin{array}{cc} Ahat{Sigma}_{k-1}A^{T}+Q & (Ahat{Sigma}_{k-1}A^{T}+Q)H^{T}\ H(Ahat{Sigma}_{k-1}A^{T}+Q)^{T} & H(Ahat{Sigma}_{k-1}A^{T}+Q)H^{T}+R end{array} right])

P(xk,yky1,y2,,yk1)Nn+m([Aμ^k1+Buk1H(Au^k1+Buk1)],[AΣ^k1AT+QH(AΣ^k1AT+Q)T(AΣ^k1AT+Q)HTH(AΣ^k1AT+Q)HT+R])

P

(

x

k

,

y

k

y

1

,

y

2

,

,

y

k

1

)

P(x_{k},y_{k}|y_{1},y_{2},dots,y_{k-1})

P(xk,yky1,y2,,yk1)这个概率分布函数后，如何求

P

(

x

k

y

1

,

y

2

,

,

y

k

)

P(x_{k}|y_{1},y_{2},dots,y_{k})

P(xky1,y2,,yk),下面给定引理：

(

u

v

)

n

+

m

N

n

+

m

(

[

μ

u

μ

v

]

,

[

Σ

u

Σ

u

v

Σ

v

u

Σ

v

]

)

begin{pmatrix} u\ v\ end{pmatrix}_{n+m}sim N_{n+m}( left[ begin{array}{c} mu_{u}\ mu_{v} end{array} right], left[ begin{array}{cc} Sigma_{u} & Sigma_{uv}\ Sigma_{vu} & Sigma_{v} end{array} right])

(uv)n+mNn+m([μuμv],[ΣuΣvuΣuvΣv])

P

(

u

v

)

N

n

(

μ

u

+

Σ

u

v

Σ

v

1

(

v

μ

v

)

,

Σ

u

Σ

u

v

Σ

v

1

Σ

u

v

T

)

P(u|v)sim N_{n}(mu_{u}+Sigma_{uv}Sigma^{-1}_{v}(v-mu_{v}),Sigma_{u}-Sigma_{uv}Sigma^{-1}_{v}Sigma^{T}_{uv})

P(uv)Nn(μu+ΣuvΣv1(vμv),ΣuΣuvΣv1ΣuvT)

P

(

x

k

,

y

k

y

1

,

y

2

,

,

y

k

1

)

P(x_{k},y_{k}|y_{1},y_{2},dots,y_{k-1})

P(xk,yky1,y2,,yk1)，根据以上引理，可以得到

P

(

x

k

y

1

,

y

2

,

,

y

k

)

(

A

μ

^

k

1

+

B

u

k

1

+

(

A

Σ

^

k

1

A

T

+

Q

)

H

T

[

H

(

A

Σ

^

k

1

A

T

+

Q

)

H

T

+

R

]

1

[

y

k

H

(

A

μ

^

k

1

+

B

u

k

1

)

]

,

(

A

Σ

^

k

1

A

T

+

Q

)

(

A

Σ

^

k

1

A

T

+

Q

)

H

T

[

H

(

A

Σ

^

k

1

A

T

+

Q

)

H

T

+

R

]

1

H

(

A

Σ

^

k

1

A

T

+

Q

)

T

)

P(x_{k}|y_{1},y_{2},dots,y_{k})sim(Ahat{mu}_{k-1}+Bu_{k-1}+(Ahat{Sigma}_{k-1}A^{T}+Q)H^{T}[H(Ahat{Sigma}_{k-1}A^{T}+Q)H^{T}+R]^{-1}[y_{k}-H(Ahat{mu}_{k-1}+Bu_{k-1})],(Ahat{Sigma}_{k-1}A^{T}+Q)-(Ahat{Sigma}_{k-1}A^{T}+Q)H^{T}[H(Ahat{Sigma}_{k-1}A^{T}+Q)H^{T}+R]^{-1}H(Ahat{Sigma}_{k-1}A^{T}+Q) ^{T})

P(xky1,y2,,yk)(Aμ^k1+Buk1+(AΣ^k1AT+Q)HT[H(AΣ^k1AT+Q)HT+R]1[ykH(Aμ^k1+Buk1)],(AΣ^k1AT+Q)(AΣ^k1AT+Q)HT[H(AΣ^k1AT+Q)HT+R]1H(AΣ^k1AT+Q)T)

x

^

k

=

A

μ

^

k

1

+

B

u

k

1

hat{x}_{overline{k}}=Ahat{mu}_{k-1}+Bu_{k-1}

x^k=Aμ^k1+Buk1

P

k

=

A

Σ

^

k

1

A

T

+

Q

P_{overline{k}}=Ahat{Sigma}_{k-1}A^{T}+Q

Pk=AΣ^k1AT+Q

K

k

=

P

k

H

T

[

H

P

k

H

T

+

R

]

1

K_{k}=P_{overline{k}}H^{T}[HP_{overline{k}}H^{T}+R]^{-1}

Kk=PkHT[HPkHT+R]1,则

P

(

x

k

y

1

,

y

2

,

,

y

k

)

P(x_{k}|y_{1},y_{2},dots,y_{k})

P(xky1,y2,,yk)可以被重写成

P

(

x

k

y

1

,

y

2

,

,

y

k

)

(

x

^

k

+

K

k

(

y

k

H

x

^

k

)

,

P

k

K

k

H

P

k

)

P(x_{k}|y_{1},y_{2},dots,y_{k})sim(hat{x}_{overline{k}}+K_{k}(y_{k}-Hhat{x}_{overline{k}}),P_{overline{k}}-K_{k}HP_{overline{k}})

P(xky1,y2,,yk)(x^k+Kk(ykHx^k),PkKkHPk)

P

(

x

k

y

1

,

y

2

,

,

y

k

)

(

x

^

k

+

K

k

(

y

k

H

x

^

k

)

,

P

k

K

k

H

P

k

)

P(x_{k}|y_{1},y_{2},dots,y_{k})sim(hat{x}_{overline{k}}+K_{k}(y_{k}-Hhat{x}_{overline{k}}),P_{overline{k}}-K_{k}HP_{overline{k}})

P(xky1,y2,,yk)(x^k+Kk(ykHx^k),PkKkHPk)，为了形式上和

P

(

x

k

1

y

1

,

y

2

,

,

y

k

1

)

N

n

(

μ

^

k

1

,

Σ

^

k

1

)

P(x_{k-1}|y_{1},y_{2},dots,y_{k-1})sim N_{n}(hat{mu}_{k-1},hat{Sigma}_{k-1})

P(xk1y1,y2,,yk1)Nn(μ^k1,Σ^k1)一致，我们记

μ

^

k

=

x

^

k

+

K

k

(

y

k

H

x

^

k

)

hat{mu}_{k}=hat{x}_{overline{k}}+K_{k}(y_{k}-Hhat{x}_{overline{k}})

μ^k=x^k+Kk(ykHx^k)

Σ

^

k

=

P

k

K

k

H

P

k

=

(

I

K

k

H

)

P

k

hat{Sigma}_{k}=P_{overline{k}}-K_{k}HP_{overline{k}}=(I-K_{k}H)P_{overline{k}}

Σ^k=PkKkHPk=(IKkH)Pk

P

(

x

k

y

1

,

y

2

,

,

y

k

)

N

n

(

μ

^

k

,

Σ

^

k

)

P(x_{k}|y_{1},y_{2},dots,y_{k})sim N_{n}(hat{mu}_{k},hat{Sigma}_{k})

P(xky1,y2,,yk)Nn(μ^k,Σ^k)，所以在当前时间

t

=

k

t=k

t=k时，在发生了测量值

y

1

,

y

2

,

,

y

k

y_{1},y_{2},dots,y_{k}

y1,y2,,yk的情况下，系统真实状态

x

k

x_{k}

xk

μ

^

k

hat{mu}_{k}

μ^k的概率最大，我们也说

μ

^

k

hat{mu}_{k}

μ^k是系统在

t

=

k

t=k

t=k时真实状态

x

k

x_{k}

xk的最优估计。

t

=

k

1

t=k-1

t=k1时系统的状态

x

k

1

x_{k-1}

xk1符合

P

(

x

k

1

y

1

,

y

2

,

,

y

k

1

)

N

n

(

μ

^

k

1

,

Σ

^

k

1

)

P(x_{k-1}|y_{1},y_{2},dots,y_{k-1})sim N_{n}(hat{mu}_{k-1},hat{Sigma}_{k-1})

P(xk1y1,y2,,yk1)Nn(μ^k1,Σ^k1)，即系统在

t

=

k

1

t=k-1

t=k1时的真实状态

x

k

1

x_{k-1}

xk1的最优估计是

μ

^

k

1

hat{mu}_{k-1}

μ^k1。现在根据

P

(

x

k

1

y

1

,

y

2

,

,

y

k

1

)

N

n

(

μ

^

k

1

,

Σ

^

k

1

)

P(x_{k-1}|y_{1},y_{2},dots,y_{k-1})sim N_{n}(hat{mu}_{k-1},hat{Sigma}_{k-1})

P(xk1y1,y2,,yk1)Nn(μ^k1,Σ^k1)可以推算出

t

=

k

t=k

t=k时系统的状态

x

k

x_{k}

xk符合

P

(

x

k

1

y

1

,

y

2

,

,

y

k

)

N

n

(

μ

^

k

,

Σ

^

k

)

P(x_{k-1}|y_{1},y_{2},dots,y_{k})sim N_{n}(hat{mu}_{k},hat{Sigma}_{k})

P(xk1y1,y2,,yk)Nn(μ^k,Σ^k)

x

^

k

=

A

μ

^

k

1

+

B

u

k

1

hat{x}_{overline{k}}=Ahat{mu}_{k-1}+Bu_{k-1}

x^k=Aμ^k1+Buk1

P

k

=

A

Σ

^

k

1

A

T

+

Q

P_{overline{k}}=Ahat{Sigma}_{k-1}A^{T}+Q

Pk=AΣ^k1AT+Q

K

k

=

P

k

H

T

[

H

P

k

H

T

+

R

]

1

K_{k}=P_{overline{k}}H^{T}[HP_{overline{k}}H^{T}+R]^{-1}

Kk=PkHT[HPkHT+R]1

μ

^

k

=

x

^

k

+

K

k

(

y

k

H

x

^

k

)

hat{mu}_{k}=hat{x}_{overline{k}}+K_{k}(y_{k}-Hhat{x}_{overline{k}})

μ^k=x^k+Kk(ykHx^k)

Σ

^

k

=

P

k

K

k

H

P

k

=

(

I

K

k

H

)

P

k

hat{Sigma}_{k}=P_{overline{k}}-K_{k}HP_{overline{k}}=(I-K_{k}H)P_{overline{k}}

Σ^k=PkKkHPk=(IKkH)Pk

t

=

k

1

t=k-1

t=k1，在系统观测量

y

1

,

y

2

,

,

y

k

1

y_{1},y_{2},dots,y_{k-1}

y1,y2,,yk1已知的情况下，根据系统状态

x

k

1

x_{k-1}

xk1的条件概率密度函数，来计算在

t

=

k

t=k

t=k时，且系统观测量

y

1

,

y

2

,

,

y

k

y_{1},y_{2},dots,y_{k}

y1,y2,,yk已知的情况下，系统状态

x

k

x_{k}

xk的条件概率密度函数。如下图所示，

x

x

x

x

:

2

×

1

x:2times 1

x:2×1，即系统状态有两个分量

x

(

1

)

x^{(1)}

x(1)

x

(

2

)

x^{(2)}

x(2)，系统的条件密度函数由服从

N

2

(

μ

^

k

1

,

Σ

^

k

1

)

N_{2}(hat{mu}_{k-1},hat{Sigma}_{k-1})

N2(μ^k1,Σ^k1)的多元高斯分布变成了另一个服从

N

2

(

μ

^

k

,

Σ

^

k

)

N_{2}(hat{mu}_{k},hat{Sigma}_{k})

N2(μ^k,Σ^k)的多元高斯分布。

# 卡尔曼滤波器的相关基础知识

B站徐亦达老师卡尔曼滤波器讲解视频

# 卡尔曼滤波器的仿真

%%% kalman filter %%%
% the state space function is as follows
% x_{k} = Ax_{k-1}+Bu_{k-1}+ w_{k-1}
% where Cov(ww^{T}) = Q . Matrix size: A: n×n  B: n×p
% the measurement function is as follows
% z_{k} = Hx_{k} + v_{k}
% where Cov(vv^{T}) = R . Matrix size: H: m×n

% k=1:   P(x_{1}|y_{1})=N(posterior probability{u_{1}},posterior probability{sigma_{1}})
% k=2:   P(x_{2}|y_{1})=N(prior probability{u_{2}},prior probability{sigma_{2}})
%        P(x_{2}|y_{1},y_{2})=N(posterior probability{u_{2}},posterior probability{sigma_{2}})
% k=t:   P(x_{k}|y_{1},y_{2},...,y_{k})=N(prior probability{u_{k}},prior probability{sigma_{k}})
%        P(x_{k}|y_{1},y_{2},...,y_{k})=N(posterior probability{u_{k}},posterior probability{sigma_{k}})

A = [0, 0.1, 0; 0, 0.2, 0; 0, 0, 0.3];
B = [0.1; 0.1; 0.1];
H = [1, 0, 0;0, 1, 0;0, 0, 1];
Q = [0.01, 0, 0; 0, 0.01, 0; 0, 0, 0.01]; % 注意这里我仿真设置的协方差矩阵中，噪声的每一个分量都是不相关的，实际中不同噪声分量可以相关。
R = [0.09, 0, 0;0, 0.09, 0;0, 0, 0.09]; % 注意这里我仿真设置的协方差矩阵中，噪声的每一个分量都是不相关的，实际中不同噪声分量可以相关。

posterior_probability_u=[0;0;0]; % 时间步为1时的初始化状态期望
posterior_probability_sigma=[0,0,0;0,0,0;0,0,0]; % 初始时刻t=1时,初始化状态协方差矩阵
real_state=zeros(3,1000);
estimate_state=zeros(3,1000);
input_u=ones(1,1000);
measurement_value=zeros(3,1000);
estimate_state(:,1)=posterior_probability_u;
t=150;
for i = 2:t
a=normrnd(0,0.1,[3,1]);
real_state(:,i)=A*real_state(:,i-1)+B*input_u(:,i-1)+a;
measurement_value(:,i)=H*real_state(:,i)+normrnd(0,0.3,[3,1]);
prior_probability_u=A*estimate_state(:,i-1)+B*input_u(:,i-1);
prior_probability_sigma=A*posterior_probability_sigma*A'+Q;
kalman_gain=prior_probability_sigma*H'/(H*prior_probability_sigma*H'+R);
estimate_state(:,i)=prior_probability_u+kalman_gain*(measurement_value(:,i)-H*prior_probability_u);
posterior_probability_sigma=(eye(3,3)-kalman_gain*H)*prior_probability_sigma;
end
figure(1);
plot(real_state(1,1:t),'r');
hold on;
plot(measurement_value(1,1:t),'y');
hold on;
plot(estimate_state(1,1:t),'b');
legend('real state','mesurement value','estimate state');

figure(2);
plot(real_state(2,1:t),'r');
hold on;
plot(measurement_value(2,1:t),'y');
hold on;
plot(estimate_state(2,1:t),'b');
legend('real state','mesurement value','estimate state');


t

=

1

t=1

t=1时，

P

(

x

1

y

1

)

N

3

(

μ

^

1

,

Σ

^

1

)

P(x_{1}|y_{1})sim N_{3}(hat{mu}_{1},hat{Sigma}_{1})

P(x1y1)N3(μ^1,Σ^1)中的

μ

^

1

hat{mu}_{1}

μ^1对应的是posterior_probability_u

Σ

^

1

hat{Sigma}_{1}

Σ^1对应的是posterior_probability_sigma，这是我们事前根据自己对系统的了解设置的，在上述程序中的系统的真实初始值

x

1

=

[

0

;

0

;

0

]

x_{1}=[0;0;0]

x1=[0;0;0],当我们把posterior_probability_u初始化为

[

0

;

0

;

0

]

[0;0;0]

[0;0;0]时，对于系统状态的第一个分量

x

(

1

)

x^{(1)}

x(1)仿真结果如下图，

R

R

R的改为R = [0.0001, 0, 0;0, 0.0001, 0;0, 0, 0.0001],仿真结果如下，红线和黄线基本重合，反而卡尔曼滤波器估计出来的最优值不如测量值靠谱。

t

=

1

t=1

t=1时，系统初始

P

(

x

1

y

1

)

N

3

(

μ

^

1

,

Σ

^

1

)

P(x_{1}|y_{1})sim N_{3}(hat{mu}_{1},hat{Sigma}_{1})

P(x1y1)N3(μ^1,Σ^1)中的

μ

^

1

hat{mu}_{1}

μ^1对应的是posterior_probability_u，在上述程序中的系统的真实初始值

x

1

=

[

0

;

0

;

0

]

x_{1}=[0;0;0]

x1=[0;0;0],当我们把posterior_probability_u初始化为

[

1

;

1

;

1

]

[1;1;1]

[1;1;1]时，这个设置和系统真实情况出入很大，对于系统状态的第一个分量

x

(

1

)

x^{(1)}

x(1)仿真结果如下图，

# 卡尔曼滤波器的总结

t

=

k

1

t=k-1

t=k1的状态值得到

t

=

k

t=k

t=k时，系统状态值大概值（概率分布）。第二条线是我们有系统的传感器传回来的测量值（大多时候传感器的测量噪声比较大）。根据这两条线，来计算系统最可能的状态值（即最优估计

μ

^

k

hat{mu}_{k}

μ^k）。

THE END