卡尔曼滤波

最新推荐文章于 2024-05-19 21:45:19 发布

娃哈哈纯净李

最新推荐文章于 2024-05-19 21:45:19 发布

阅读量4.1k

点赞数

分类专栏：滤波器文章标签：卡尔曼滤波器

滤波器专栏收录该内容

2 篇文章 0 订阅

订阅专栏

下面转自：http://blog.csdn.net/newthinker_wei/article/details/11768443

过程方程：

X(k+1) = A X(k) + B U(k) + W(k) >>>>式1

量测方程：

Z(k+1) = H X（k+1）+ V(k+1) >>>>式2

A和B是系统参数，对于多模型系统，他们为矩阵；H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声，他们的协方差分别是Q，R。为了不失一般性，下面的讨论中将X,Z都视为矩阵，其中X是m行的单列矩阵，Z是n行的单列矩阵。

说明：下面的表达式中，不带前缀的量都代表实际量，其小括号里面的“k”或“k+1”代表该量是第k或第k+1时刻的实际量，如“Z(k+1)”就代表第k+1时刻的实际测量值；

带前缀“^”的量都代表预测量，如果小括号里面是“k+1|k”，就代表k+1时刻的先验预测值，如果小括号里面是“k+1|k+1”，就代表k+1时刻的后验预测值；（测量值可以通过测量得到，所以只有先验预测，没有后验预测。而实际状态值无法得知，既有先验预测，又有后验预测）

带前缀“~”的量都代表与预测值对应的偏差值。

实际状态值与先验预测状态值的偏差 = 实际状态值 – 先验预测状态值

~X(k+1|k) = X(k+1) - ^X(k+1|k) >>>> 式3

实际测量值与先验预测测量值的偏差 = 当前测量值 - 先验预测测量值

~Z(k+1|k) = Z(k+1) - ^Z(k+1|k) >>>>式4

并且

先验预测测量值 = 转换矩阵H * 先验预测状态值

^Z(k+1|k) = H ^X(k+1|k) >>>> 式5

得到测量值后，再对当前状态值X(k+1) 进行后验预测（设后验预测值为 ^Z(k+1|k+1) ），则后验预测值（同时也是最终预测值）的偏差为

~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1) >>>>式6

为了得到当前状态值X(k+1)，根据式3，需要：

X(k+1) = ^X(k+1|k) + ~X(k+1|k) >>>> 式7

上式中，我们可以通过卡尔曼公式1（见附注2）计算出^X(k+1|k)，但我们无法得知实际状态值X(k+1)，因而~X(k+1|k) 也无法得知。我们最终的目的是得出一个比较接近实际状态值X(k+1)的滤波值^X(k+1|k+1)，根据式7，只要能准确的估计出~X(k+1|k)即可。

~X(k+1|k)本身虽无法得知，但~Z(k+1|k) 却可以通过测量得到，而且它们二者存在一定的相关性。不妨再设存在一个矩阵K（m行n列矩阵），能使得

~X(k+1|k) = K * ~Z(k+1|k) >>>>式8

那么最终的预测任务其实就是找到K。由于~X(k+1|k)和~Z(k+1|k)都是单列矩阵，因此不难看出，满足式8的矩阵K应有无穷多个。矩阵K中第i行第j列反映了量测变量偏差矩阵~Z(k+1|k)的第j个元素对状态变量偏差矩阵~X(k+1|k)的第i个元素的贡献。因此矩阵K的物理意义很明显，K的第i行第j列的元素表示：对于第i个待测的状态量来说，第j个测量仪器测到的偏差的可信度。某个测量值对应的可信度越高，滤波器越“相信”该测量值。

既然满足条件的K有无穷多个，那应该使用哪个K呢？实际上，我们并不知道~X(k+1|k)的值，所以也就无法直接计算出K，而只能通过某种方法找到一个Kg，使得将Kg带入式8后，等号两边的差（的平方）的期望尽可能小。

我们最终的预测值或滤波值是后验预测值^X(k+1|k+1)，因此最后的预测也应使 ~X(k+1|k+1) 的期望为0且方差最小（这与让8式两端的差最小是一致的，下面的式9体现了这一点），这样预测值才最可靠。下面详细说明。

^X(k+1|k+1) = ^X(k+1|k) + Kg * ~Z(k+1|k) （后验预测的状态值）

~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1) （后验预测的偏差）

~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1)

= ( ^X(k+1|k) + ~X(k+1|k) ) - ( ^X(k+1|k) + Kg * ~Z(k+1|k) )

= ~X(k+1|k) - Kg * ~Z(k+1|k)

>>>>式9

~Z(k+1|k) = Z(k+1) - ^Z(k+1|k)

= ( H X（k+1）+ V(k+1) ) - ( H ^X(k+1|k) )

= H ( X（k+1）-^X(k+1|k) ) + V(k+1)

= H ~ X(k+1|k) + V(k+1) >>>>式10

接下来的分析中，为了更直观的说明卡尔曼滤波的原理，我们用几何方法来解释。这时，~X和~Z矩阵中的每个元素应看做向量空间中的一个向量而不再是一个单纯的数。这个向量空间（统计测试空间）可以看成无穷多维的，每一个维对应一个可能的状态。~X和~Z矩阵中的每个元素向量都是由所有可能的状态按照各自出现的概率组合而成（在测量之前，~X和~Z 的实际值都是不可知的）。~X和~Z中的每个元素向量都应是0均值的，他们与自己的内积就是他们的协方差矩阵。我们无法给出~X和~Z中每个元素向量的具体表达，但我们通过协方差矩阵就可以知道所有元素向量的模长，以及相互之间的夹角（从内积计算）。

为了方便用几何方法解释，我们假设状态变量X是一个1行1列的矩阵（即只有一个待测状态量），而量测变量Z是一个2行1列的矩阵（即有两个测量仪器，共同测量同一个状态量X），也就是说，m=1，n=2。矩阵X中只有X[1]一项，矩阵Z中有Z[1]和Z[2]两项。Kg此时应是一个1行2列的矩阵，两个元素分别记作Kg1 和 Kg2 。H和V此时应是一个2行1列的矩阵。

将矩阵表达式9和10按元素展开：

~X(k+1|k+1)[1] = ~X(k+1|k)[1] - (Kg1 * ~Z(k+1|k)[1] + Kg2 * ~Z(k+1|k)[2] ) >>>> 式9i

~Z(k+1|k)[i] = H[i] ~ X(k+1|k) + V(k+1)[i] >>>>式10i

~X(k+1|k) 中各个元素（向量）的线性组合可以产生一个m维或更低维的向量子空间Vx，这里，按照我们的假设，m=1，所以Vx应是一维的；同时V(k+1)中的各个元素（向量）的线性组合也可以产生一个n维或更低维的向量子空间Vv，这里，按照我们的假设，n=2，所以Vv应是二维的。由于V(k+1)中的每一项与~X(k+1|k)中的每一项都不相关（见附注1），故这两个子空间相互垂直。如下图所示。式10i所体现的~Z(k+1|k)[i]、H[i]~ X(k+1|k)、V(k+1)[i] 三者之间的几何关系，也在下图中描绘了出来。

从上图中可以看出，~Z(k+1|k)中各个元素（向量）的线性组合也可以产生一个n维或更低维的向量子空间Vz，这里已假设n=2，所以Vz是一个二维的平面，就是上图中两条红色的线所构成的平面。

通过先验预测的协方差矩阵（见卡尔曼公式2），可以得到~X(k+1|k)中各个元素的模长以及彼此间的夹角。这是因为协方差矩阵中的第i行第j列其实就代表了~X(k+1|k)中第i个元素向量与第j个元素向量的内积。

通过测量可以得到新息协方差（见卡尔曼公式3的分母部分），进而可以知道~Z(k+1|k)中各个元素的模长以及彼此间的夹角。

通过已知的量测噪声协方差矩阵R，可以得出V(k+1) 中各个元素的模长以及彼此间的夹角。

最后根据~X(k+1|k+1)[1]与Y垂直以及图1中所示的几何关系，用高中学的立体几何和向量知识就可以求得两个Kg的值了。如果将向量的内积都用协方差矩阵表示，就会发现，我们最后求得的Kg，其实就是卡尔曼公式3。

（上面讨论的是较低次的卡尔曼滤波，只有一个待测量，两个测量仪器。这种情况还是比较常见的，比如倾角测量系统中，我们用加速度计和陀螺仪共同测量倾角。对于更高次的卡尔曼滤波，X和Z都是多行矩阵时，用几何方法已经无法直观解释，只能用矩阵分析的方法证明。求解Kg的详细过程参考《卡尔曼滤波器及其应用基础》国防工业出版社敬喜编）

卡尔曼滤波的核心过程，就是求解能使得

E { ~X(k+1|k+1) ’ * ~X(k+1|k+1) }

取最小值的Kg增益矩阵的过程，~X(k+1|k+1)’代表的是~X(k+1|k+1)的转置（这里~X(k+1|k+1)中的元素代表数值，不是向量）。前面已经提到过，卡尔曼增益矩阵Kg中的元素，都代表测量仪器测到的偏差的可信度，或者叫估计权重。

附注1：

(a). v(k+1)中的每一项与~X(k+1|k)中的每一项都不相关

~X(k+1|k) = X(k+1) - ^X(k+1|k)

= X(k+1) - ( A ^X(k|k) + B U(k) )

= ( A X(k) + B U(k) + W(k) ) - ( A X(k) + B U(k) - A ~X(k|k) )

= W(k) + A ~X(k|k)

= W(k) + A ( ~X(k|k-1) - Kg(k)* ~ Z(k|k-1) ) -----这一步利用了式9

= W(k) + A ( ~X(k|k-1) - Kg(k)* ( H ~X(k|k-1) + v(k) ) ) ------这一步利用了式10

= W(k) - A Kg(k) *v(k) + A ( I - Kg(k) * H ) ~X(k|k-1)

上式最后一行出现了~X(k|k-1)，可见~X(k+1|k)可以递归表示。而且递归式中的过程噪声W(k)与v(k+1)不相关，同时由于 v本身是白噪声，所以 v(k+1)与v(k)亦不相关（白噪声的自相关是δ函数），因此通过递推式可以判断v(k+1)与~X(k+1|k)不相关。

(b). w(k+1)中的每一项与~X(k+1|k+1)中的每一项都不相关，w(k+1)中的每一项与~X(k+1|k)中的每一项都不相关。

~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1)

= ( ^X(k+1|k) + ~X(k+1|k) ) - ( ^X(k+1|k) + Kg(k+1)*~Z(k+1|k) )

= ~X(k+1|k) - Kg(k+1)*~Z(k+1|k)

= ~X(k+1|k) - Kg(k+1)* ( H ~X(k+1|k) + v(k+1) )

= - Kg(k+1)* v(k+1) + ( I - Kg(k+1) * H ) ~X(k+1|k)

我们已经知道w(k+1)与v(k+1)不相关，因此只要~X(k+1|k+1)与上式的第二项也不相关，就说明结论(b)成立。根据(a)中的结论，~X(k+1|k)的递归展开式中出现的 v(k) ，w(k) ， v(k-1)，w(k-1)等等，显然 w(k+1)与 v (m=k，k-1……) 都不相关，另外，由于w(k+1)的自相关为δ函数，因此w(k+1)与 w(m=k，k-1……) 也不相关，也就得出w(k+1)与~X(k+1|k) 不相关。

进而可知，w(k+1)与~X(k+1|k+1)不相关。

正是因为 (a) (b)中的两个不相关，卡尔曼公式中的预测协方差矩（卡尔曼公式(2)）和新息协方差矩阵（卡尔曼公式(3)中的“分母”部分）才可以是简单的加式。

附注2：卡尔曼滤波的五个公式

先验预测值与先验预测协方差矩阵的计算。求解协方差时，都认为预测值的期望是实际值。因此，^X(k+1|k)的协方差矩阵同样也是 ~X(k+1|k) 的协方差矩阵，又因为偏差~X(k+1|k)的期望是0，因此协方差矩阵反映了~X(k+1|k)在向量空间中的模长。注意，协方差矩阵都是对称矩阵。

X(k+1|k)=A X(k|k)+B U(k) (1)

P(k+1|k)=A P(k|k) A’+Q(k) (2)

卡尔曼增益矩阵的计算。量测预测值为

Z(k+1|k) = H X(k+1|k)

新息协方差见公式（3）中的“分母”部分。量测预测值的期望是实际量测值。因此，^Z(k+1|k)的协方差矩阵同样也是 ~Z(k+1|k) 的协方差矩阵，又因为偏差~Z(k+1|k)的期望是0，因此协方差矩阵反映了~Z(k+1|k)在向量空间中的模长。

Kg(k+1)= P(k+1|k) H’/ (H P(k+1|k) H’ + R(k+1)) (3)

后验预测值与后验预测协方差矩阵的计算

X(k+1|k+1)= X(k+1|k)+Kg(k+1) (Z(k+1)-H X(k+1|k)) (4)

P(k+1|k+1)=（I-Kg(k+1) H）P(k+1|k) (5)

下面的转自：http://zh.wikipedia.org/wiki/卡尔曼滤波

应用实例[编辑]

卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，通过对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。在很多工程应用（如雷达、计算机视觉）中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题。

例如，对于雷达来说，人们感兴趣的是其能够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计（滤波），也可以是对于将来位置的估计（预测），也可以是对过去位置的估计（插值或平滑）。

命名[编辑]

这种滤波方法以它的发明者鲁道夫.E.卡尔曼（Rudolph E. Kalman）命名，但是根据文献可知实际上Peter Swerling在更早之前就提出了一种类似的算法。

斯坦利。施密特（Stanley Schmidt）首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑便使用了这种滤波器。关于这种滤波器的论文由Swerling（1958）、Kalman (1960)与Kalman and Bucy（1961）发表。

目前，卡尔曼滤波已经有很多不同的实现。卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器。除此以外，还有施密特扩展滤波器、信息滤波器以及很多Bierman, Thornton开发的平方根滤波器的变种。也许最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。

以下的讨论需要线性代数以及概率论的一般知识。

基本动态系统模型[编辑]

卡尔曼滤波建立在线性代数和隐马尔可夫模型（hidden Markov model）上。其基本动态系统可以用一个马尔可夫链表示，该马尔可夫链建立在一个被高斯噪声（即正态分布的噪声）干扰的线性算子上的。系统的状态可以用一个元素为实数的向量表示。随着离散时间的每一个增加，这个线性算子就会作用在当前状态上，产生一个新的状态，并也会带入一些噪声，同时系统的一些已知的控制器的控制信息也会被加入。同时，另一个受噪声干扰的线性算子产生出这些隐含状态的可见输出。

为了从一系列有噪声的观察数据中用卡尔曼滤波器估计出被观察过程的内部状态，我们必须把这个过程在卡尔曼滤波的框架下建立模型。也就是说对于每一步k，定义矩阵F_k, H_k, Q_k, R_k，有时也需要定义B_k，如下。

卡尔曼滤波器的模型。圆圈代表向量，方块代表矩阵，星号代表高斯噪声，其协方差矩阵在右下方标出。

卡尔曼滤波模型假设k时刻的真实状态是从（k − 1）时刻的状态演化而来，符合下式：

$\textbf{x}_{k} = \textbf{F}_{k} \textbf{x}_{k-1} + \textbf{B}_{k}\textbf{u}_{k-1} + \textbf{w}_{k-1}$

其中

F_k是作用在x_k−1上的状态变换模型（/矩阵/矢量）。
B_k是作用在控制器向量u_k上的输入－控制模型。
w_k是过程噪声，并假定其符合均值为零，协方差矩阵为Q_k的多元正态分布。

\textbf{w}_{k} \sim N(0, \textbf{Q}_k)

时刻k，对真实状态x_k的一个测量z_k满足下式：

\textbf{z}_{k} = \textbf{H}_{k} \textbf{x}_{k} + \textbf{v}_{k}

其中H_k是观测模型，它把真实状态空间映射成观测空间，v_k是观测噪声，其均值为零，协方差矩阵为R_k,且服从正态分布。

\textbf{v}_{k} \sim N(0, \textbf{R}_k)

初始状态以及每一时刻的噪声{x₀, w₁, ..., w_k, v₁ ... v_k}都认为是互相独立的。

实际上，很多真实世界的动态系统都并不确切的符合这个模型；但是由于卡尔曼滤波器被设计在有噪声的情况下工作，一个近似的符合已经可以使这个滤波器非常有用了。更多其它更复杂的卡尔曼滤波器的变种，在下边讨论中有描述。

卡尔曼滤波器[编辑]

卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器与大多数滤波器不同之处，在于它是一种纯粹的时域滤波器，它不需要像低通滤波器等频域滤波器那样，需要在频域设计再转换到时域实现。

卡尔曼滤波器的状态由以下两个变量表示：

$\hat{\textbf{x}}_{k|k}$ ，在时刻k的状态的估计；
$\textbf{P}_{k|k}$ ，误差相关矩阵，度量估计值的精确程度。

卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。

预测[编辑]

\hat{\textbf{x}}_{k|k-1} = \textbf{F}_{k}\hat{\textbf{x}}_{k-1|k-1} + \textbf{B}_{k} \textbf{u}_{k}

(预测状态)

\textbf{P}_{k|k-1} = \textbf{F}_{k} \textbf{P}_{k-1|k-1} \textbf{F}_{k}^{T} + \textbf{Q}_{k}

(预测估计协方差矩阵）

可参考：http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

更新[编辑]

首先要算出以下三个量：

\tilde{\textbf{y}}_{k} = \textbf{z}_{k} - \textbf{H}_{k}\hat{\textbf{x}}_{k|k-1}

(测量余量，measurement residual)

\textbf{S}_{k} = \textbf{H}_{k}\textbf{P}_{k|k-1}\textbf{H}_{k}^{T} + \textbf{R}_{k}

(测量余量协方差)

\textbf{K}_{k} = \textbf{P}_{k|k-1}\textbf{H}_{k}^{T}\textbf{S}_{k}^{-1}

(最优卡尔曼增益）

然后用它们来更新滤波器变量x与P：

\hat{\textbf{x}}_{k|k} = \hat{\textbf{x}}_{k|k-1} + \textbf{K}_{k}\tilde{\textbf{y}}_{k}

(更新的状态估计)

\textbf{P}_{k|k} =(I - \textbf{K}_{k} \textbf{H}_{k}) \textbf{P}_{k|k-1}

(更新的协方差估计）

使用上述公式计算 $\textbf{P}_{k|k}$ 仅在最优卡尔曼增益的时候有效。使用其他增益的话，公式要复杂一些，请参见推导。

不变量（Invariant）[编辑]

如果模型准确，而且 $\hat{\textbf{x}}_{0|0}$ 与 $\textbf{P}_{0|0}$ 的值准确的反映了最初状态的分布，那么以下不变量就保持不变：所有估计的误差均值为零

$\textrm{E}[\textbf{x}_k - \hat{\textbf{x}}_{k|k}] = \textrm{E}[\textbf{x}_k - \hat{\textbf{x}}_{k|k-1}] = 0$
$\textrm{E}[\tilde{\textbf{y}}_k] = 0$

且协方差矩阵准确的反映了估计的协方差：

$\textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_k - \hat{\textbf{x}}_{k|k})$
$\textbf{P}_{k|k-1} = \textrm{cov}(\textbf{x}_k - \hat{\textbf{x}}_{k|k-1})$
$\textbf{S}_{k} = \textrm{cov}(\tilde{\textbf{y}}_k)$

请注意，其中 $\textrm{E}[\textbf{a}]$ 表示 ${a}$ 的期望值, $\textrm{cov}(\textbf{a}) = \textrm{E}[\textbf{a}\textbf{a}^T]$ 。

实例[编辑]

考虑在无摩擦的、无限长的直轨道上的一辆车。该车最初停在位置0处，但时不时受到随机的冲击。我们每隔Δt秒即测量车的位置，但是这个测量是非精确的；我们想建立一个关于其位置以及速度的模型。我们来看如何推导出这个模型以及如何从这个模型得到卡尔曼滤波器。

因为车上无动力，所以我们可以忽略掉B_k和u_k。由于F、H、R和Q是常数，所以时间下标可以去掉。

车的位置以及速度（或者更加一般的，一个粒子的运动状态）可以被线性状态空间描述如下：

\textbf{x}_{k} = \begin{bmatrix} x \\ \dot{x} \end{bmatrix}

其中 $\dot{x}$ 是速度，也就是位置对于时间的导数。

我们假设在（k − 1）时刻与k时刻之间，车受到a_k的加速度，其符合均值为0，标准差为σ_a的正态分布。根据牛顿运动定律，我们可以推出

\textbf{x}_{k} = \textbf{F} \textbf{x}_{k-1} + \textbf{G}a_{k}

其中

\textbf{F} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}

且

\textbf{G} = \begin{bmatrix} \frac{\Delta t^{2}}{2} \\ \Delta t \end{bmatrix}

我们可以发现

\textbf{Q} = \textrm{cov}(\textbf{G}a) = \textrm{E}[(\textbf{G}a)(\textbf{G}a)^{T}] = \textbf{G} \textrm{E}[a^2] \textbf{G}^{T} = \textbf{G}[\sigma_a^2]\textbf{G}^{T} = \sigma_a^2 \textbf{G}\textbf{G}^{T}

（因为 σ _a是一个标量）。

在每一时刻，我们对其位置进行测量，测量受到噪声干扰。我们假设噪声服从正态分布，均值为0，标准差为σ_z。

\textbf{z}_{k} = \textbf{H x}_{k} + \textbf{v}_{k}

其中

\textbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix}

且

\textbf{R} = \textrm{E}[\textbf{v}_k \textbf{v}_k^{T}] = \begin{bmatrix} \sigma_z^2 \end{bmatrix}

如果我们知道足够精确的车最初的位置，那么我们可以初始化

\hat{\textbf{x}}_{0|0} = \begin{bmatrix} 0 \\ 0 \end{bmatrix}

并且，我们告诉滤波器我们知道确切的初始位置，我们给出一个协方差矩阵：

\textbf{P}_{0|0} = \begin{bmatrix} 0 & 0 \\ 0 & 0 \end{bmatrix}

如果我们不确切的知道最初的位置与速度，那么协方差矩阵可以初始化为一个对角线元素是B的矩阵，B取一个合适的比较大的数。

\textbf{P}_{0|0} = \begin{bmatrix} B & 0 \\ 0 & B \end{bmatrix}

此时，与使用模型中已有信息相比，滤波器更倾向于使用初次测量值的信息。

推导[编辑]

推导后验协方差矩阵[编辑]

按照上边的定义，我们从误差协方差 $\textbf{P}_{k|k}$ 开始推导如下：

\textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_{k} - \hat{\textbf{x}}_{k|k})

代入 $\hat{\textbf{x}}_{k|k}$

\textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_{k} - (\hat{\textbf{x}}_{k|k-1} + \textbf{K}_k\tilde{\textbf{y}}_{k}))

再代入 $\tilde{\textbf{y}}_k$

\textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_{k} - (\hat{\textbf{x}}_{k|k-1} + \textbf{K}_k(\textbf{z}_k - \textbf{H}_k\hat{\textbf{x}}_{k|k-1})))

与 $\textbf{z}_{k}$

\textbf{P}_{k|k} = \textrm{cov}(\textbf{x}_{k} - (\hat{\textbf{x}}_{k|k-1} + \textbf{K}_k(\textbf{H}_k\textbf{x}_k + \textbf{v}_k - \textbf{H}_k\hat{\textbf{x}}_{k|k-1})))

整理误差向量，得

\textbf{P}_{k|k} = \textrm{cov}((I - \textbf{K}_k \textbf{H}_{k})(\textbf{x}_k - \hat{\textbf{x}}_{k|k-1}) - \textbf{K}_k \textbf{v}_k )

因为测量误差v_k与其他项是非相关的，因此有

\textbf{P}_{k|k} = \textrm{cov}((I - \textbf{K}_k \textbf{H}_{k},\textbf{x}_k - \hat{\textbf{x}}_{k|k-1})) + \textrm{cov}(\textbf{K}_k \textbf{v}_k )

利用协方差矩阵的性质，此式可以写作

\textbf{P}_{k|k} =(I - \textbf{K}_k \textbf{H}_{k})\textrm{cov}(\textbf{x}_k - \hat{\textbf{x}}_{k|k-1},I - \textbf{K}_k \textbf{H}_{k})^{T} + \textbf{K}_k\textrm{cov}(\textbf{v}_k )\textbf{K}_k^{T}

使用不变量P_k|k-1以及R_k的定义这一项可以写作： $\textbf{P}_{k|k}=(I - \textbf{K}_k \textbf{H}_{k}) \textbf{P}_{k|k-1}(I - \textbf{K}_k \textbf{H}_{k})^T +\textbf{K}_k \textbf{R}_k \textbf{K}_k^T$ 这一公式对于任何卡尔曼增益K_k都成立。如果K_k是最优卡尔曼增益，则可以进一步简化，请见下文。

最优卡尔曼增益的推导[编辑]

卡尔曼滤波器是一个最小均方误差估计器，后验状态误差估计（英文：a posteriori state estimate）是

\textbf{x}_{k} - \hat{\textbf{x}}_{k|k}

我们最小化这个矢量幅度平方的期望值， $\textrm{E}[|\textbf{x}_{k} - \hat{\textbf{x}}_{k|k}|^2]$ ，这等同于最小化后验估计协方差矩阵P_k|k的迹（trace）。将上面方程中的项展开、抵消，得到：

$\textbf{P}_{k\|k}$	$= \textbf{P}_{k\|k-1} - \textbf{K}_k \textbf{H}_k \textbf{P}_{k\|k-1} - \textbf{P}_{k\|k-1} \textbf{H}_k^T \textbf{K}_k^T + \textbf{K}_k(\textbf{H}_k \textbf{P}_{k\|k-1} \textbf{H}_k^T + \textbf{R}_k) \textbf{K}_k^T$
	$= \textbf{P}_{k\|k-1} - \textbf{K}_k \textbf{H}_k \textbf{P}_{k\|k-1} - \textbf{P}_{k\|k-1} \textbf{H}_k^T \textbf{K}_k^T + \textbf{K}_k \textbf{S}_k\textbf{K}_k^T$

当矩阵导数是0的时候得到P_k|k的迹（trace）的最小值：

\frac{d \; \textrm{tr}(\textbf{P}_{k|k})}{d \;\textbf{K}_k} = -2(\textbf{H}_k \textbf{P}_{k|k-1})^T + 2 \textbf{K}_k \textbf{S}_k = 0

此处须用到一个常用的式子，如下：

    $\frac{d \; \textrm{tr}(\textbf{BAC})}{d \;\textbf{A}} = B^TC^T$

从这个方程解出卡尔曼增益K_k：

\textbf{K}_k \textbf{S}_k =(\textbf{H}_k \textbf{P}_{k|k-1})^T = \textbf{P}_{k|k-1} \textbf{H}_k^T

\textbf{K}_{k} = \textbf{P}_{k|k-1} \textbf{H}_k^T \textbf{S}_k^{-1}

这个增益称为最优卡尔曼增益，在使用时得到最小均方误差。

后验误差协方差公式的化简[编辑]

在卡尔曼增益等于上面导出的最优值时，计算后验协方差的公式可以进行简化。在卡尔曼增益公式两侧的右边都乘以S_kK_k^T得到

\textbf{K}_k \textbf{S}_k \textbf{K}_k^T = \textbf{P}_{k|k-1} \textbf{H}_k^T \textbf{K}_k^T

根据上面后验误差协方差展开公式，

\textbf{P}_{k|k} = \textbf{P}_{k|k-1} - \textbf{K}_k \textbf{H}_k \textbf{P}_{k|k-1} - \textbf{P}_{k|k-1} \textbf{H}_k^T \textbf{K}_k^T + \textbf{K}_k \textbf{S}_k \textbf{K}_k^T

最后两项可以抵消，得到

\textbf{P}_{k|k} = \textbf{P}_{k|k-1} - \textbf{K}_k \textbf{H}_k \textbf{P}_{k|k-1} =(I - \textbf{K}_{k} \textbf{H}_{k}) \textbf{P}_{k|k-1}

这个公式的计算比较简单，所以实际中总是使用这个公式，但是需注意这公式仅在使用最优卡尔曼增益的时候它才成立。如果算术精度总是很低而导致数值稳定性出现问题，或者特意使用非最优卡尔曼增益，那么就不能使用这个简化；必须使用上面导出的后验误差协方差公式。

与递归贝叶斯估计之间的关系[编辑]

假设真正的状态是无法观察的马尔可夫过程，测量结果是从隐性马尔可夫模型观察到的状态。

Hidden Markov Model

根据马尔可夫假设，真正的状态仅受最近一个状态影响而与其它以前状态无关。

p(\textbf{x}_k|\textbf{x}_0,\dots,\textbf{x}_{k-1}) = p(\textbf{x}_k|\textbf{x}_{k-1})

与此类似，在时刻k测量只与当前状态有关而与其它状态无关。

p(\textbf{z}_k|\textbf{x}_0,\dots,\textbf{x}_{k}) = p(\textbf{z}_k|\textbf{x}_{k} )

根据这些假设，隐性马尔可夫模型所有状态的概率分布可以简化为：

p(\textbf{x}_0,\dots,\textbf{x}_k,\textbf{z}_1,\dots,\textbf{z}_k) = p(\textbf{x}_0)\prod_{i=1}^k p(\textbf{z}_i|\textbf{x}_i)p(\textbf{x}_i|\textbf{x}_{i-1})

然而，当卡尔曼滤波器用来估计状态x的时候，我们感兴趣的机率分布，是基于目前为止所有个测量值来得到的当前状态之机率分布

p(\textbf{x}_k|\textbf{Z}_{k-1}) = \int p(\textbf{x}_k | \textbf{x}_{k-1}) p(\textbf{x}_{k-1} | \textbf{Z}_{k-1} ) \, d\textbf{x}_{k-1}

信息滤波器[编辑]

非线性滤波器[编辑]

基本卡尔曼滤波器（The basic Kalman filter）是限制在线性的假设之下。然而，大部份非平凡的（non-trivial）的系统都是非线性系统。其中的“非线性性质”（non-linearity）可能是伴随存在过程模型（process model）中或观测模型（observation model）中，或者两者兼有之。

扩展卡尔曼滤波器[编辑]

在扩展卡尔曼滤波器（Extended Kalman Filter，简称EKF）中状态转换和观测模型不需要是状态的线性函数，可替换为（可微的）函数。

\textbf{x}_{k} = f(\textbf{x}_{k-1}, \textbf{u}_{k-1}, \textbf{w}_{k-1})

\textbf{z}_{k} = h(\textbf{x}_{k}, \textbf{v}_{k})

函数f可以用来从过去的估计值中计算预测的状态，相似的，函数h可以用来以预测的状态计算预测的测量值。然而f和h不能直接的应用在协方差中，取而代之的是计算偏导矩阵（Jacobian）。

在每一步中使用当前的估计状态计算Jacobian矩阵，这几个矩阵可以用在卡尔曼滤波器的方程中。这个过程，实质上将非线性的函数在当前估计值处线性化了。

这样一来，卡尔曼滤波器的等式为：

预测

\hat{\textbf{x}}_{k|k-1} = f(\textbf{x}_{k-1}, \textbf{u}_{k}, 0)

\textbf{P}_{k|k-1} = \textbf{F}_{k} \textbf{P}_{k-1|k-1} \textbf{F}_{k}^{T} + \textbf{Q}_{k}

使用Jacobians矩阵更新模型

\textbf{F}_{k} = \left . \frac{\partial f}{\partial \textbf{x} } \right \vert _{\hat{\textbf{x}}_{k-1|k-1},\textbf{u}_{k}}

\textbf{H}_{k} = \left . \frac{\partial h}{\partial \textbf{x} } \right \vert _{\hat{\textbf{x}}_{k|k-1}}

更新

\tilde{\textbf{y}}_{k} = \textbf{z}_{k} - h(\hat{\textbf{x}}_{k|k-1}, 0)

\textbf{S}_{k} = \textbf{H}_{k}\textbf{P}_{k|k-1}\textbf{H}_{k}^{T} + \textbf{R}_{k}

\textbf{K}_{k} = \textbf{P}_{k|k-1}\textbf{H}_{k}^{T}\textbf{S}_{k}^{-1}

\hat{\textbf{x}}_{k|k} = \hat{\textbf{x}}_{k|k-1} + \textbf{K}_{k}\tilde{\textbf{y}}_{k}

\textbf{P}_{k|k} =(I - \textbf{K}_{k} \textbf{H}_{k}) \textbf{P}_{k|k-1}

预测

如同扩展卡尔曼滤波器（EKF）一样, UKF的预测过程可以独立于UKF的更新过程之外，与一个线性的（或者确实是扩展卡尔曼滤波器的）更新过程合并来使用；或者，UKF的预测过程与更新过程在上述中地位互换亦可。

娃哈哈纯净李

关注

0
点赞
踩
5

收藏

觉得还不错? 一键收藏
0
评论
卡尔曼滤波

下面转自：http://blog.csdn.net/newthinker_wei/article/details/11768443过程方程：X(k+1) = A X(k) + B U(k) + W(k) >>>>式1量测方程：Z(k+1) = H X（k+1）+ V(k+1) >>>>式2
复制链接

扫一扫