最新卡尔曼滤波推导_卡尔曼微分方程，已收藏

网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。

需要这份系统化的资料的朋友，可以添加戳这里获取

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！

k

1

′

x

k

\begin{aligned}\hat{x}_{k+1}-x_{k+1} \ &=K’_{k+1}\hat{x}_k+K_{k+1} H_{k+1} x_{k+1}+v_{k+1} - x_{k+1}\ &=K’_{k+1}\hat{x}_k+K_{k+1} H_{k+1} x_{k+1}+v_{k+1} - x_{k+1}-K’_{k+1}x_k+ K’_{k+1}x_k \ \end{aligned}

x^{k+1​−xk+1​​=Kk+1′​x}k​+Kk+1​Hk+1​xk+1​+vk+1​−xk+1​=Kk+1′​x^k​+Kk+1​Hk+1​xk+1​+vk+1​−xk+1​−Kk+1′​xk​+Kk+1′​xk​​
化简得：

x

^

k

1

−

x

k

1

=

K

k

1

′

(

x

^

k

−

x

k

)

(

K

k

1

H

k

1

Φ

k

1

−

Φ

k

1

K

k

1

′

)

x

k

(

K

k

1

H

k

1

−

I

)

w

k

1

K

k

1

v

k

\begin{aligned}\hat{x}_{k+1}-x_{k+1} \ = &K’_{k+1}( \hat{x}_k-x_k)+( K_{k+1}H_{k+1}\Phi_{k+1}-\Phi_{k+1}+K’_{k+1}) x_k\ &+( K_{k+1}H_{k+1}-I) w_{k+1} +K_{k+1}v_k\ \end{aligned}

x^{k+1​−xk+1​=​Kk+1′​(x}k​−xk​)+(Kk+1​Hk+1​Φk+1​−Φk+1​+Kk+1′​)xk​+(Kk+1​Hk+1​−I)wk+1​+Kk+1​vk​​

由

E

[

x

^

k

1

−

x

k

1

]

=

0

E[\hat{x}_{k+1}-x_{k+1}]=0

E[x^k+1​−xk+1​]=0易知：

E

[

x

^

k

1

−

x

k

1

]

=

[

K

k

1

H

k

1

Φ

k

1

−

Φ

k

1

K

k

1

′

]

E

[

x

k

]

=

0

E[\hat{x}_{k+1}-x_{k+1}] = [ K_{k+1}H_{k+1}\Phi_{k+1}-\Phi_{k+1}+K’_{k+1}]E[x_k] = 0

E[x^k+1​−xk+1​]=[Kk+1​Hk+1​Φk+1​−Φk+1​+Kk+1′​]E[xk​]=0

K

k

1

H

k

1

Φ

k

1

−

Φ

k

1

K

k

1

′

=

0

K_{k+1}H_{k+1}\Phi_{k+1}-\Phi_{k+1}+K’_{k+1} = 0

Kk+1​Hk+1​Φk+1​−Φk+1​+Kk+1′​=0
表明：

K

k

1

′

=

(

I

−

K

k

1

H

k

1

)

Φ

k

1

K’_{k+1}= ( I-K_{k+1}H_{k+1}) \Phi_{k+1}

Kk+1′​=(I−Kk+1​Hk+1​)Φk+1​
结论：

x

^

k

1

=

(

I

−

K

k

1

H

k

1

)

Φ

k

1

x

^

k

K

k

1

z

k

1

\hat{x}_{k+1} = ( I-K_{k+1}H_{k+1}) \Phi_{k+1}\hat{x}_k+K_{k+1}z_{k+1}

x^{k+1​=(I−Kk+1​Hk+1​)Φk+1​x}k​+Kk+1​zk+1​
或：

x

^

k

1

=

Φ

k

1

x

^

k

K

k

1

(

z

k

1

−

H

k

1

Φ

k

1

x

^

k

)

\hat{x}_{k+1} = \Phi_{k+1}\hat{x}_k+K_{k+1}( z_{k+1}-H_{k+1}\Phi_{k+1}\hat{x}_k)

x^{k+1​=Φk+1​x}k​+Kk+1​(zk+1​−Hk+1​Φk+1​x^k​)

###Step.2
建立使误差项

x

~

k

1

=

x

^

k

1

−

x

k

1

\widetilde{x}_{k+1} = \hat{x}_{k+1} -x_{k+1}

x

k+1​=x^k+1​−xk+1​的协方差矩阵

P

k

1

P_{k+1}

Pk+1​的迹最小的

K

k

1

K_{k+1}

Kk+1​的表达式
####A.
由

x

~

k

1

−

=

x

^

k

1

−

−

x

k

1

\widetilde{x}_{k+1}^- = \hat{x}_{k+1}^- -x_{k+1}

x

k+1−​=x^k+1−​−xk+1​：

x

~

k

1

−

=

x

^

k

1

−

−

x

k

1

=

Φ

k

1

x

^

k

−

Φ

k

1

x

k

w

k

1

\widetilde{x}_{k+1}^- = \hat{x}_{k+1}^- -x_{k+1} = \Phi_{k+1}\hat{x}_k-\Phi_{k+1} x_k+w_{k+1}

x

k+1−​=x^{k+1−​−xk+1​=Φk+1​x}k​−Φk+1​xk​+wk+1​

x

~

k

1

−

=

x

^

k

1

−

−

x

k

1

=

Φ

k

1

(

x

^

k

−

x

k

)

w

k

1

\widetilde{x}_{k+1}^- = \hat{x}_{k+1}^- -x_{k+1} = \Phi_{k+1}( \hat{x}_k- x_k)+w_{k+1}

x

k+1−​=x^{k+1−​−xk+1​=Φk+1​(x}k​−xk​)+wk+1​
式中

x

^

k

1

−

−

x

k

1

\hat{x}_{k+1}^- -x_{k+1}

x^k+1−​−xk+1​的协方差为

P

k

1

−

P_{k+1}^-

Pk+1−​，

x

^

k

−

x

k

\hat{x}_k- x_k

x^k​−xk​的协方差为

P

k

P_k

Pk​，

w

k

1

w_{k+1}

wk+1​的协方差为

Q

k

1

Q_{k+1}

Qk+1​。
则可推出方程2

P

k

1

−

=

E

{

x

~

k

1

−

x

~

k

1

−

T

}

=

Φ

k

1

E

{

(

x

~

k

−

x

k

)

(

x

~

k

−

x

k

)

T

}

Φ

k

1

T

E

{

w

k

1

w

k

1

T

}

=

Φ

k

1

P

k

Φ

k

1

T

Q

k

1

\begin{aligned}\ P_{k+1}^- &=E{\widetilde{x}_{k+1}^{-\widetilde{x}_{k+1}}{-T}} \ &=\Phi_{k+1}E{(\widetilde{x}_k-x_k)(\widetilde{x}_k-x_k)^T}\Phi_{k+1}T+E{w_{k+1}w_{k+1}^T}\ &=\Phi_{k+1} P_k\Phi_{k+1}^T+Q_{k+1}\ \ \end{aligned}

Pk+1−​​=E{x

k+1−​x

k+1−T​}=Φk+1​E{(x

k​−xk​)(x

k​−xk​)T}Φk+1T​+E{wk+1​wk+1T​}=Φk+1​Pk​Φk+1T​+Qk+1​​
####B.
由Step.1中：

x

^

k

1

=

Φ

k

1

x

^

k

K

k

1

(

z

k

1

−

H

k

1

Φ

k

1

x

^

k

)

\hat{x}_{k+1} = \Phi_{k+1}\hat{x}_k+K_{k+1}( z_{k+1}-H_{k+1}\Phi_{k+1}\hat{x}_k)

x^{k+1​=Φk+1​x}k​+Kk+1​(zk+1​−Hk+1​Φk+1​x^k​)
和$ \hat{x}{k+1}^- = \Phi{k+1}\hat{x}_k$得：

x

^

k

1

=

x

^

k

1

−

K

k

1

(

z

k

1

−

H

k

1

x

^

k

1

−

)

\hat{x}_{k+1} = \hat{x}_{k+1}^- +K_{k+1}( z_{k+1}-H_{k+1}\hat{x}_{k+1}^- )

x^k+1​=xk+1−​+Kk+1​(zk+1​−Hk+1​x^k+1−​)
带入

x

~

k

1

=

x

^

k

1

−

x

k

1

\widetilde{x}_{k+1} = \hat{x}_{k+1} -x_{k+1}

x

k+1​=x^k+1​−xk+1​中化简得：

x

~

k

1

=

[

I

−

K

k

1

H

k

1

]

x

~

k

1

−

−

K

k

1

v

k

1

\widetilde{x}_{k+1} = [I-K_{k+1}H_{k+1}]\widetilde{x}_{k+1}^- -K_{k+1}v_{k+1}

x

k+1​=[I−Kk+1​Hk+1​]x

k+1−​−Kk+1​vk+1​
等式两端同时取期望：

P

k

1

=

E

[

x

~

k

1

x

~

k

1

T

]

=

[

I

−

K

k

1

H

k

1

]

P

k

1

−

[

I

−

K

k

1

H

k

1

]

T

K

k

1

R

k

1

K

k

1

T

\begin{aligned}\ P_{k+1}\ &= E[\widetilde{x}_{k+1} \widetilde{x}_{k+1}^T]\ &=[I-K_{k+1}H_{k+1}]P_{k+1}^{-[I-K_{k+1}H_{k+1}]}T+K_{k+1}R_{k+1}K_{k+1}^T \ \end{aligned}

Pk+1​​=E[x

k+1​x

k+1T​]=[I−Kk+1​Hk+1​]Pk+1−​[I−Kk+1​Hk+1​]T+Kk+1​Rk+1​Kk+1T​​
####C.
推导

K

k

1

K_{k+1}

Kk+1​的表达式使

P

k

1

P_{k+1}

Pk+1​的迹最小
使用

P

P

P代替

P

k

1

−

P_{k+1}^-

Pk+1−​,

K

K

K代替

K

k

1

K_{k+1}

Kk+1​,

R

R

R代替

R

k

1

R_{k+1}

Rk+1​得:

P

k

1

=

(

I

−

K

H

)

P

(

I

−

K

H

)

T

K

R

K

T

P_{k+1}= (I-KH)P(I-KH)^T+KRKT

Pk+1​=(I−KH)P(I−KH)T+KRKT

P

k

1

=

P

−

K

H

P

−

P

H

T

K

T

K

H

P

H

T

K

R

K

T

P_{k+1}=P-KHP-PH^TKT+KHPH^T+KRKT

Pk+1​=P−KHP−PHTKT+KHPHT+KRKT
由

P

=

P

T

P=P^T

P=PT，

(

P

H

T

K

T

)

T

=

K

H

P

(PH^TKT)^T=KHP

(PHTKT)T=KHP，

T

r

(

P

H

T

K

T

)

=

T

r

(

K

H

P

)

Tr(PH^TKT)=Tr(KHP)

Tr(PHTKT)=Tr(KHP):

T

r

P

k

1

=

T

r

P

−

2

T

r

K

H

P

T

r

K

(

H

P

H

T

)

K

T

T

r

K

R

K

T

TrP_{k+1}=TrP-2TrKHP+TrK(HPH^T)KT+TrKRK^T

TrPk+1​=TrP−2TrKHP+TrK(HPHT)KT+TrKRKT

δ

T

r

P

k

1

δ

K

=

−

2

P

H

T

2

K

H

P

网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。

需要这份系统化的资料的朋友，可以添加戳这里获取

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！

1

]

T

K

k

1

R

k

1

K

k

1

T

\begin{aligned}\ P_{k+1}\ &= E[\widetilde{x}_{k+1} \widetilde{x}_{k+1}^T]\ &=[I-K_{k+1}H_{k+1}]P_{k+1}^{-[I-K_{k+1}H_{k+1}]}T+K_{k+1}R_{k+1}K_{k+1}^T \ \end{aligned}

Pk+1​​=E[x

k+1​x

k+1T​]=[I−Kk+1​Hk+1​]Pk+1−​[I−Kk+1​Hk+1​]T+Kk+1​Rk+1​Kk+1T​​
####C.
推导

K

k

1

K_{k+1}

Kk+1​的表达式使

P

k

1

P_{k+1}

Pk+1​的迹最小
使用

P

P

P代替

P

k

1

−

P_{k+1}^-

Pk+1−​,

K

K

K代替

K

k

1

K_{k+1}

Kk+1​,

R

R

R代替

R

k

1

R_{k+1}

Rk+1​得:

P

k

1

=

(

I

−

K

H

)

P

(

I

−

K

H

)

T

K

R

K

T

P_{k+1}= (I-KH)P(I-KH)^T+KRKT

Pk+1​=(I−KH)P(I−KH)T+KRKT

P

k

1

=

P

−

K

H

P

−

P

H

T

K

T

K

H

P

H

T

K

R

K

T

P_{k+1}=P-KHP-PH^TKT+KHPH^T+KRKT

Pk+1​=P−KHP−PHTKT+KHPHT+KRKT
由

P

=

P

T

P=P^T

P=PT，

(

P

H

T

K

T

)

T

=

K

H

P

(PH^TKT)^T=KHP

(PHTKT)T=KHP，

T

r

(

P

H

T

K

T

)

=

T

r

(

K

H

P

)

Tr(PH^TKT)=Tr(KHP)

Tr(PHTKT)=Tr(KHP):

T

r

P

k

1

=

T

r

P

−

2

T

r

K

H

P

T

r

K

(

H

P

H

T

)

K

T

T

r

K

R

K

T

TrP_{k+1}=TrP-2TrKHP+TrK(HPH^T)KT+TrKRK^T

TrPk+1​=TrP−2TrKHP+TrK(HPHT)KT+TrKRKT

δ

T

r

P

k

1

δ

K

=

−

2

P

H

T

2

K

H

P

[外链图片转存中…(img-i2r7aPvM-1715877358716)]
[外链图片转存中…(img-B1WO5wre-1715877358716)]

网上学习资料一大堆，但如果学到的知识不成体系，遇到问题时只是浅尝辄止，不再深入研究，那么很难做到真正的技术提升。

需要这份系统化的资料的朋友，可以添加戳这里获取

一个人可以走的很快，但一群人才能走的更远！不论你是正从事IT行业的老鸟或是对IT行业感兴趣的新人，都欢迎加入我们的的圈子（技术交流、学习资源、职场吐槽、大厂内推、面试辅导），让我们一起学习成长！