卡尔曼滤波原理详解
推荐阅读:卡尔曼滤波器介绍
卡尔曼滤波在最有估计方面真的是非常常用,当然也是非常优秀的。这次我们从基本原理出发来理解卡尔曼滤波。包括卡尔曼滤波的前置知识:协方差矩阵,最大似然估计等。
先验概率和后验概率
先验概率是指根据以往经验和分析得到的概率,如全概率公式 P ( B ) = ∑ i = 1 n P ( A i ) P ( B ∣ A i ) P ( B ) = \sum _ { i = 1 } ^ { n } P \left( A _ { i } \right) P ( B | A _ { i } ) P(B)=∑i=1nP(Ai)P(B∣Ai) ,它往往作为“由因求果”问题中的“因”出现。意思是说我们人有一个常识,比如骰子,我们都知道概率是1/6,而且无数次重复实验也表明是这个数,这是一种我们人的常识,也是我们在不知道任何情况下必然会说出的一个值.而所谓的先验概率是我们人在未知条件下对事件发生可能性猜测的数学表示。
后验概率是指在得到“结果”的信息后重新修正的概率,是“执果寻因”问题中的“因” ,如贝叶斯公式 P ( x ∣ z ) = P ( z ∣ x ) P ( x ) P ( z ) ∝ P ( z ∣ x ) P ( x ) P ( \boldsymbol { x } | \boldsymbol { z } ) = \frac { P ( \boldsymbol { z } | \boldsymbol { x } ) P ( \boldsymbol { x } ) } { P ( \boldsymbol { z } ) } \propto P ( \boldsymbol { z } | \boldsymbol { x } ) P ( \boldsymbol { x } ) P(x∣z)=P(z)P(z∣x)P(x)∝P(z∣x)P(x)。后验概率是基于新的信息,修正原来的先验概率后所获得的更接近实际情况的概率估计。后验概率 : 事情已经发生,要求这件事情发生的原因是由某个因素引起的可能性的大小。
先验概率和后验概率是相对的。如果以后还有新的信息引入,更新了现在所谓的后验概率,得到了新的概率值,那么这个新的概率值被称为后验概率。
协方差和协方差矩阵
协方差
协方差是对两个随机变量 X X X, Y Y Y联合分布线性相关程度的一种度量。两个随机变量越线性相关,协方差越大,完全线性无关,协方差为零。
cov ( X , Y ) = ∑ i = 1 n ( X i − X ‾ ) ( Y i − Y ‾ ) n − 1 \operatorname { cov } ( X , Y ) = \frac { \sum _ { i = 1 } ^ { n } \left( X _ { i } - \overline { X } \right) \left( Y _ { i } - \overline { Y } \right) } { n - 1 } cov(X,Y)=n−1∑i=1n(Xi−X)(Yi−Y)
协方差矩阵
由于协方差反应的是两个变量之间的相关性,因此,协方差矩阵表示的是所有变量之间两两相关的关系,具体来讲,一个包含两个特征的矩阵,其协方差矩阵应该有 2×2 大小:
Cov ( Z ) = [ Cov ( X , X ) Cov ( X , Y ) Cov ( Y , X ) Cov ( Y , Y ) ] \operatorname { Cov } ( Z ) = \left[ \begin{array} { c c } { \operatorname { Cov } ( X , X ) } & { \operatorname { Cov } ( X , Y ) } \\ { \operatorname { Cov } ( Y , X ) } & { \operatorname { Cov } ( Y , Y ) } \end{array} \right] Cov(Z)=[Cov(X,X)Cov(Y,X)Cov(X,Y)Cov(Y,Y)]
独立变量的协方差
如果两个变量 X, Y 独立,那么它们的协方差 Cov ( X , Y ) = 0 \operatorname { Cov } ( X , Y ) = 0 Cov(X,Y)=0,这时,协方差矩阵就变成一个对角矩阵了:
Cov ( Z ) = [ Cov ( X , X ) 0 0 Cov ( Y , Y ) ] \operatorname { Cov } ( Z ) = \left[ \begin{array} { c c } { \operatorname { Cov } ( X , X ) } & { 0 } \\ { 0 } & { \operatorname { Cov } ( Y , Y ) } \end{array} \right] Cov(Z)=[Cov(X,X)00Cov(Y,Y)]
n个变量的协方差矩阵
多个变量的协方差矩阵和上面一样,特别的,对多维随机变量 X = [ X 1 , X 2 , X 3 , … , X n ] T \mathbf { X } = \left[ X _ { 1 } , X _ { 2 } , X _ { 3 } , \dots , X _ { n } \right] ^ { T } X=[X1,X2,X3,…,Xn]T,我们往往需要计算各维度两两之间的协方差,这样各协方差组成了一个n×n的协方差矩阵。协方差矩阵是个对称矩阵,对角线上的元素是各维度上随机变量的方差。我们定义协方差矩阵为 Σ Σ Σ,这个符号与求和 Σ Σ Σ相同,需要根据上下文区分。矩阵内的元素 Σ i j \Sigma _ { i j } Σij为:
Σ i j = cov ( X i , X j ) = E [ ( X i − E [ X i ] ) ( X j − E [ X j ] ) ] \Sigma _ { i j } = \operatorname { cov } \left( X _ { i } , X _ { j } \right) = \mathrm { E } \left[ \left( X _ { i } - \mathrm { E } \left[ X _ { i } \right] \right) \left( X _ { j } - \mathrm { E } \left[ X _ { j } \right] \right) \right] Σij=cov(Xi,Xj)=E[(Xi−E[Xi])(Xj−E[Xj])]
这样这个矩阵为:
Σ = E [ ( X − E [ X ] ) ( X − E [ X ] ) T ] \Sigma = \mathrm { E } \left[ ( \mathbf { X } - \mathrm { E } [ \mathbf { X } ] ) ( \mathbf { X } - \mathrm { E } [ \mathbf { X } ] ) ^ { T } \right] Σ=E[(X−E[X])(X−E[X])T]
或者:
[ cov ( X 1 , X 1 ) cov ( X 1 , X 2 ) ⋯ cov ( X 1 , X n ) cov ( X 2 , X 1 ) cov ( X 2 , X 2 ) ⋯ cov ( X 2 , X n ) ⋮ ⋮ ⋱ ⋮ cov ( X n , X 1 ) cov ( X n , X 2 ) ⋯ cov ( X n , X n ) ] \left[ \begin{array} { c c c c } { \operatorname { cov } \left( X _ { 1 } , X _ { 1 } \right) } & { \operatorname { cov } \left( X _ { 1 } , X _ { 2 } \right) } & { \cdots } & { \operatorname { cov } \left( X _ { 1 } , X _ { n } \right) } \\ { \operatorname { cov } \left( X _ { 2 } , X _ { 1 } \right) } & { \operatorname { cov } \left( X _ { 2 } , X _ { 2 } \right) } & { \cdots } & { \operatorname { cov } \left( X _ { 2 } , X _ { n } \right) } \\ { \vdots } & { \vdots } & { \ddots } & { \vdots } \\ { \operatorname { cov } \left( X _ { n } , X _ { 1 } \right) } & { \operatorname { cov } \left( X _ { n } , X _ { 2 } \right) } & { \cdots } & { \operatorname { cov } \left( X _ { n } , X _ { n } \right) } \end{array} \right] ⎣⎢⎢⎢⎡cov(X1,X1)cov(X2,X1)⋮cov(Xn,X1)