卡尔曼滤波 从推导到应用1
卡尔曼滤波的推导
卡尔曼滤波的推导卡尔曼滤波的推导1 最⼩⼆乘法在⼀个线性系统中，若\(x\)为常量，是我们要估计的量，关于\(x\)的观测⽅程如下：\[y = Hx + v \tag{1.1} \]\(H\)是观测矩阵（或者说算符），\(v\)是噪⾳，\(y\)是观察量。
若关于\(x\)的最佳估计为\(\hat{x}\),误差可定义为\(\epsilon_y\):\[\epsilon_y = y - H\hat{x} \tag{1.2} \]定义代价函数\(J\)为：\[J = \epsilon_{y1}^2 + \epsilon_{y2}^2 + ... + \epsilon_{yk}^2 \tag{1.3} \]将⽅程\((1.2)\)代⼊\((1.3)\)可得：\[J=\left(y-H\hat{x}\right)^T\left(y-H\hat{x}\right)=y^Ty-x^TH^Ty-y^TH\hat{x}+\hat{x}^TH^TH\hat{x} \tag{1.4} \]使代价函数\(J\)最⼩的就是最佳估计，对\(J\)求偏导，并使之为0：\[\frac{\partial J}{\partial \hat{x}}=-2y^TH+2\hat{x}^TH^TH=0 \tag{1.5} \]解得：\[\hat{x} = \left(H^TH\right)^{-1}H^Ty \tag{1.6} \]2 递推最⼩⼆乘法线性系统的递推估计值可以写成以下形式：\[y_k=H_kx+v_k \tag{2.1} \]\[\hat{x}_k = \hat{x}_{k-1} + K_k\left(y_k-H_k\hat{x}_{k-1}\right) \tag{2.2} \]其中是\(K_k\)待定的增益矩阵，\(y_k-H_k\hat{x}_{k-1}\)为修正项。
先考虑线性递推估计器的估计误差均值,要注意的是这⾥和式\((1.2)\)有所不同，但本质都是⼀样东西：\[\begin{split} E\left(\epsilon_{x,k}\right) &= E\left(x-\hat{x}_k\right) \cr &= E\left(x- \hat{x}_{k-1}-K_k\left(y_k-H_k\hat{x}_{k-1}\right)\right)\newline &= E\left(\epsilon_{x,k-1}-K_k\left(H_kx+v_k-H_k\hat{x}_{k-1}\right)\right) \cr &= E\left(\epsilon_{x,k-1}-K_kH_k\left(x-\hat{x}_{k-1}\right)-K_kv_k\right) \cr &=\left(I-K_kH_k\right)E\left(\epsilon_{x,k-1}\right)-K_kE\left(v_k\right) \end{split} \tag{2.3} \]类似地，代价函数\(J_k\)表达为：\[J_k=E(\epsilon_{x1,k}^2+\epsilon_{x2,k}^2+...+\epsilon_{xn,k}^2)=E(\epsilon_{x,k}^T\epsilon_{x,k})=E(Tr(\epsilon_{x,k}\epsilon_{x,k}^T))= Tr(P_k) \tag{2.4} \]其中\(P_k\)为估计误差的协⽅差矩阵，利⽤式\((2.3)\)可以得到：\[\begin{split} P_k &= E\left(\epsilon_{x,k}\epsilon_{x,k}^T\right) \newline &=E\left\{\left[(I-K_kH_k)E(\epsilon_{x,k-1})-K_kE(v_k)\right]\left[(I-K_kH_k)E(\epsilon_{x,k-1})-K_kE(v_k)\right]^T\right\} \cr &=(I-K_kH_k)E(\epsilon_{x,k}\epsilon_{x,k}^T)(I-K_kH_k)^T -K_kE(v_k\epsilon_{x,k-1}^T) \cr & \quad -(I-K_kH_k)E(\epsilon_{x,k-1}v_k^T)K_k^T+K_kE(v_kv_k^T)K_k^T \end{split} \tag{2.5} \]由于\(\epsilon_{x,k-1}\)与\(v_k\)是相互独⽴的，所以有：\[E(v_k\epsilon_{x,k-1}^T)=E(v_k)E(\epsilon_{x,k-1})=0 \tag{2.6} \]令\(R_k=E(v_kv_k^T)\)为噪声的协⽅差矩阵，式\((2.5)\)变成：\[P_k = (I-K_kH_k)P_{k-1}(1-K_kH_k)^T + K_kR_kK_k^T \tag{2.7} \]我们的⽬标是求出待定的增益矩阵\(K_k\)，对\(J_k\)求偏导，并使之为0可得：\[\frac{\partial J_k}{\partial K_k}=2(I-K_kH_k)P_{k-1}(-H_k)^T+2K_kR_k = 0 \tag{2.8} \]可\(K_k\)解得以下：\[\begin{split} (I-K_kH_k)P_{k-1}H_k^T=K_kR_k \cr K_k(R_k+H_kP_{k-1}H_k^T)=P_{k-1}H_k^T \cr K_k=P_{k-1}H_k^T\left(R_k+H_kP_{k-1}H_k^T\right)^{-1} \end{split} \tag{2.9} \]递推最⼩⼆乘法总结如下:a. 初始估计值：\[\hat{x}_0 = E(x) \tag{2.10} \]\[P_0 = E\left[(x - \hat{x}_0)(x-\hat{x}_0)^T\right] \tag{2.11} \]b. 对于k=1,2,3…:假设线性系统的观测⽅程如下：\[y_k = H_kx + v_k \tag{2.12} \]其中是\(v_k\)均值为0，协⽅差矩阵为\(R_k\)的随机变量，每步测量的噪声都是相互独⽴的，则矩阵为\(R_k\)对⾓矩阵，也就是说测量的噪声为⽩噪声。
卡尔曼滤波器
Ak (xk1 xˆk1 H kCk Ak (xˆk1 xk1) k1 H kCk Akk1 H k vk
(I H kCk ) Ak (xk1 xˆk1) (I H kCk )k1 H k vk
(I H kCk ) Ak (xk1 xˆk1) k1 H kvk
(2.5.17)
精品文档
第二章 维纳滤波和卡尔曼滤波
所以(xˆskuǒ1yǐ) 仅依赖于xk-1,vk-1，而与vk不相关，即 E[(xk1 xˆk1)vkT ] E[vk (xk1 xˆk1)T ] 0 (2.5.18)
E[(xk1 xˆk1)kT1] E[k1(xk1 xˆk1)T ] 0 (2.5.19)
(2.5.24)
令
U T (Pk'CkT )T Ck Pk'T Ck Pk'
(2.5.25)
精品文档
第二章 维纳滤波和卡尔曼滤波
定义:设A∈Cn×n是Hermite矩阵,如果对任意0≠x∈Cn,都有 xHAx＞0,则A是Hermite正定阵; 若xHAx≥0,则A是Hermite半正定阵.
定理(dìnglǐ):设A∈ Cn×n 是Hermite矩阵,则下列条件等价 (1)A是Hermite矩阵,AH=A (2)A的特征值全为正实数 (3)存在矩阵P ∈Cn×n,使得A=PHP
(3) 卡尔曼滤波采取的误差准则仍为估计误差的均方值最小。
精品文档
第二章 维纳滤波和卡尔曼滤波 2.5.1 卡尔曼滤波的状态方程(fāngchéng)和量测方程(fāngchéng)
假设某系统k时刻的状态变量为xk，状态方程(fāngchéng)和量 测方程(fāngchéng)（也称为输出方程(fāngchéng)）表示为
卡尔曼滤波_卡尔曼算法
卡尔曼滤波_卡尔曼算法1.引言1.1 概述卡尔曼滤波是一种用于估计系统状态的技术，通过融合传感器测量值和系统模型的预测值，提供对系统状态的最优估计。
它的应用十分广泛，特别在导航、图像处理、机器人技术等领域中发挥着重要作用。
在现实世界中，我们往往面临着各种噪声和不确定性，这些因素会影响我们对系统状态的准确估计。
卡尔曼滤波通过动态调整系统状态的估计值，可以有效地抑制这些干扰，提供更加精确的系统状态估计。
卡尔曼滤波的核心思想是利用系统模型的预测和传感器测量值之间的线性组合，来计算系统状态的最优估计。
通过动态地更新状态估计值，卡尔曼滤波可以在对系统状态的准确估计和对传感器测量值的实时响应之间进行平衡。
卡尔曼滤波算法包括两个主要步骤：预测和更新。
在预测步骤中，通过系统模型和上一时刻的状态估计值，预测当前时刻的系统状态。
在更新步骤中，将传感器测量值与预测值进行比较，然后根据测量误差和系统不确定性的权重，计算系统状态的最优估计。
卡尔曼滤波具有很多优点，例如它对传感器噪声和系统模型误差具有鲁棒性，可以提供较为稳定的估计结果。
此外，卡尔曼滤波还可以有效地处理缺失数据和不完全的测量信息，具有较高的自适应性和实时性。
尽管卡尔曼滤波在理论上具有较好的性能，但实际应用中还需考虑诸如系统模型的准确性、测量噪声的特性等因素。
因此，在具体应用中需要根据实际情况进行算法参数的调整和优化，以提高估计的准确性和可靠性。
通过深入理解卡尔曼滤波的原理和应用，我们可以更好地应对复杂环境下的估计问题，从而在实际工程中取得更好的效果。
本文将介绍卡尔曼滤波的基本原理和算法步骤，以及其在不同领域的应用案例。
希望通过本文的阅读，读者们可以对卡尔曼滤波有一个全面的了解，并能够在实际工程中灵活运用。
1.2文章结构文章结构部分的内容可以按照以下方式编写：1.2 文章结构本文将围绕卡尔曼滤波和卡尔曼算法展开论述。
首先，我们会在引言部分对卡尔曼滤波和卡尔曼算法进行简要概述，介绍其基本原理和应用领域。
卡尔曼滤波方法
次提出的一种估计方法。之所以称为滤波，是因为它是一 种排除随机干扰，提高检测精度的一种手段。
• KF是基于最小方差准则推导出来的一种线性滤波器。 • KF是一种时域递推算法，根据上一状态的估计值和当前
状态的观测值推出当前状态，不需存储大量的历史数据， 便于计算机实现。
xˆk xˆk K( yk yˆk )
Px, k Px, k KPy, k K T
27
Xˆ k|k Xˆ k k1 Kk Z~k k1
测量更新 /修正
方差估值 Pk k [I Kk Hk ]Pk k1
7
3.5 卡尔曼滤波的结构图
上述递推公式，称为卡尔曼滤波器。实际上，卡尔曼 滤波器也是一个系统，其结构框图如下：
Zk + -
+
Kk
+
Z k|k 1
当前估计值
Xˆ k
14
3.7 联邦卡尔曼滤波
• 卡尔曼滤波最成功的工程应用是设计运载体的高精度组合
导航系统。为了与联邦滤波方法相区别，将普通的卡尔曼
滤波称为集中卡尔曼滤波。
• 由于对导航精度要求的提高，导航设备越来越多。另一方
面，现代系统向大系统和复杂系统的方向发展。这种情况
下采用集中式卡尔曼实现组合导航，存在两个问题：
yˆ
k
W (m) i
i
k|k 1
i0
2n
Py, k
Wi
(c)
[
i k|k
1
yˆ k
][
i k|k 1
yˆk ]T
卡尔曼滤波
式中，N 1向量 (n)表示观测数据y(n)的新的信息，简称新息。
3.2、新息过程
新息 (n)具有以下性质： 性质1 n时刻的新息 (n)与所有过去的观测数据y(1), ...，y(n-
1)正交，即：
E{(n)yH (k)} 0,1 k n 1.......(7)
二.估计原理和卡尔曼滤波
1. 状态估计原理 2. 为什么要用状态估计理论 3. 经典控制理论与现代控制理论 3. 什么是卡尔曼滤波 5.卡尔曼滤波器的应用
2.1状态估计原理
状态估计是卡尔曼滤波的重要组成部分。 一般来说，根据观测数据对随机量进行定 量推断就是估计问题，特别是对动态行为 的状态估计，它能实现实时运行状态的估 计和预测功能。比如对汽车状态估计。
在kalman滤波中，并不直接估计观测数据向量的进一步预测 ，而
是先计算状态向量的一步预测
def
x1 (n) x(n y(1),... y(n 1))........ (11)
然后再用到下式得到
y 1
(n)：
y (n) C(n) x1(n)...........(12)
1
3.2、新息过程
将上式代入新息过程的定义式（6），可得到：
卡尔曼滤波控制系统结构图
由于系统的状态x是不确定的，卡尔曼滤波器 的任务就是在有随机干扰w和噪声v的情况下给出系
统状态x的最优估算值xˆ ，它在统计意义下最接近 状态的真值x，从而实现最优控制u( xˆ )的目的。
2.4什么是卡尔曼滤波：
卡尔曼滤波是美国工程师Kalman 在线 性最小方差估计的基础上，提出的在数学结 构上比较简单的而且是最优线性递推滤波方 法，具有计算量小、存储量低，实时性高的 优点。特别是对经历了初始滤波后的过渡状 态，滤波效果非常好。
卡尔曼滤波的基本原理及应用
卡尔曼滤波的基本原理及应用卡尔曼滤波在信号处理与系统控制领域应用广泛，目前，正越来越广泛地应用于计算机应用的各个领域。
为了更好地理解卡尔曼滤波的原理与进行滤波算法的设计工作，主要从两方面对卡尔曼滤波进行阐述：基本卡尔曼滤波系统模型、滤波模型的建立以及非线性卡尔曼滤波的线性化。
最后，对卡尔曼滤波的应用做了简单介绍。
卡尔曼滤波属于一种软件滤波方法，其基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。
最初的卡尔曼滤波算法被称为基本卡尔曼滤波算法，适用于解决随机线性离散系统的状态或参数估计问题。
卡尔曼滤波器包括两个主要过程：预估与校正。
预估过程主要是利用时间更新方程建立对当前状态的先验估计，及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计值；校正过程负责反馈，利用测量更新方程在预估过程的先验估计值及当前测量变量的基础上建立起对当前状态的改进的后验估计。
这样的一个过程，我们称之为预估－校正过程，对应的这种估计算法称为预估－校正算法。
以下给出离散卡尔曼滤波的时间更新方程和状态更新方程。
时间更新方程：状态更新方程：在上面式中，各量说明如下：A：作用在X k-1上的n×n 状态变换矩阵B：作用在控制向量U k-1上的n×1 输入控制矩阵H：m×n 观测模型矩阵，它把真实状态空间映射成观测空间P k－：为n×n 先验估计误差协方差矩阵P k：为n×n 后验估计误差协方差矩阵Q：n×n 过程噪声协方差矩阵R：m×m 过程噪声协方差矩阵I：n×n 阶单位矩阵K k：n×m 阶矩阵，称为卡尔曼增益或混合因数随着卡尔曼滤波理论的发展，一些实用卡尔曼滤波技术被提出来，如自适应滤波，次优滤波以及滤波发散抑制技术等逐渐得到广泛应用。
卡尔曼滤波算法的程序实现和推导过程
卡尔曼滤波算法的程序实现和推导过程卡尔曼滤波算法的程序实现和推导过程---蒋海林（QQ：280586940）---卡尔曼滤波算法由匈牙利裔美国数学家鲁道夫·卡尔曼（Rudolf Emil Kalman）创立，这个数学家特么牛逼，1930年出生，现在还能走能跳，吃啥啥麻麻香，但他的卡尔曼滤波算法已经广泛应用在航空航天，导弹发射，卫星在轨运行等很多高大上的应用中。
让我们一边膜拜一边上菜吧，下面就是卡尔曼滤波算法的经典程序，说是经典，因为能正常运行的程序都长得差不多，在此向原作者致敬。
看得懂的，帮我纠正文中的错误；不太懂的，也不要急，让我慢慢道来。
最后希望广大朋友转载时，能够保留我的联系方式，一则方便后续讨论共同进步，二则支持奉献支持正能量。
void Kalman_Filter(float Gyro,float Accel)///角速度，加速度{///陀螺仪积分角度（先验估计）Angle_Final = Angle_Final + (Gyro - Q_bias) * dt;///先验估计误差协方差的微分Pdot[0] = Q_angle - PP[0][1] - PP[1][0];Pdot[1] = - PP[1][1];Pdot[2] = - PP[1][1];Pdot[3] = Q_gyro;///先验估计误差协方差的积分PP[0][0] += Pdot[0] * dt;PP[0][1] += Pdot[1] * dt;PP[1][0] += Pdot[2] * dt;PP[1][1] += Pdot[3] * dt;///计算角度偏差Angle_err = Accel - Angle_Final;///卡尔曼增益计算PCt_0 = C_0 * PP[0][0];PCt_1 = C_0 * PP[1][0];E = R_angle + C_0 * PCt_0;K_0 = PCt_0 / E;K_1 = PCt_1 / E;///后验估计误差协方差计算t_0 = PCt_0;t_1 = C_0 * PP[0][1];PP[0][0] -= K_0 * t_0;PP[0][1] -= K_0 * t_1;PP[1][0] -= K_1 * t_0;PP[1][1] -= K_1 * t_1;Angle_Final += K_0 * Angle_err; ///后验估计最优角度值Q_bias += K_1 * Angle_err; ///更新最优估计值的偏差Gyro_Final = Gyro - Q_bias; ///更新最优角速度值}我们先把卡尔曼滤波的5个方程贴上来：X(k|k-1)=A X(k-1|k-1)+B U(k) ……… (1)//先验估计P(k|k-1)=A P(k-1|k-1) A ’+Q ……… (2)//协方差矩阵的预测Kg(k)= P(k|k-1) H ’ / (H P(k|k-1) H ’ + R) ……… (3)//计算卡尔曼增益 X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1)) ……… (4)通过卡尔曼增益进行修正 P(k|k)=（I-Kg(k) H ）P(k|k-1) ……… (5)//更新协方差阵这5个方程比较抽象，下面我们就来把这5个方程和上面的程序对应起来。
卡尔曼滤波推导过程
卡尔曼滤波推导过程卡尔曼滤波（Kalman Filter）是一种用于估计比实际观测更加精确的状态值的算法。
它由卡尔曼提出并在20世纪60年代的航天任务中得到广泛应用。
卡尔曼滤波的基本思想是将实际观测值和先验估计值加权平均，得到更加准确的状态估计值。
假设我们有一个系统，其状态在每个时刻t都可以用一个向量表示，记为x(t)。
该系统的状态在每个时刻会根据一个线性动态模型进行变化，而且可能受到一些噪声的影响。
我们通过一些观测来获得系统状态的估计。
预测步骤：在预测步骤中，我们需要使用系统的动态模型来预测下一个时刻的状态估计值。
假设动态模型可以表示为：x(t)=F(t)x(t-1)+B(t)u(t-1)+w(t-1)其中，F(t)是状态转移矩阵，描述系统状态如何从t-1时刻过渡到t时刻；B(t)是输入控制矩阵，描述控制量对状态变化的影响；u(t-1)是输入控制向量；w(t-1)是状态转移噪声，假设为零均值的高斯白噪声。
系统的状态估计误差可以用协方差矩阵P(t)表示。
卡尔曼滤波假设这个误差也是根据系统模型的动态演化规律进行更新。
假设误差协方差满足以下方程：P(t)=F(t)P(t-1)F(t)T+Q(t-1)其中，Q(t-1)是过程噪声协方差矩阵，也是零均值的高斯白噪声。
更新步骤：在更新步骤中，我们需要使用观测量来重新调整状态估计值和协方差矩阵。
假设我们在时刻t观测到一个向量z(t)，其与系统的状态有线性关系：z(t)=H(t)x(t)+v(t)其中，H(t)是观测矩阵，描述观测量和系统状态的关系；v(t)是观测噪声，假设为零均值的高斯白噪声。
通过比较观测值z(t)和状态估计值的预测值，我们可以计算误差的协方差矩阵S(t)：S(t)=H(t)P(t)H(t)T+R(t)其中，R(t)是观测噪声协方差矩阵，也是零均值的高斯白噪声。
卡尔曼增益K(t)可以用于权衡观测值和状态估计值的权重。
它的计算公式为：K(t)=P(t)H(t)T(S(t))^(-1)通过观测值和卡尔曼增益，我们可以更新状态估计值和协方差矩阵。
初学者的卡尔曼滤波——扩展卡尔曼滤波（一）
初学者的卡尔曼滤波——扩展卡尔曼滤波（⼀）简介 已经历经了半个世纪的卡尔曼滤波⾄今仍然是研究的热点，相关的⽂章不断被发表。
其中许多⽂章是关于卡尔曼滤波器的新应⽤，但也不乏改善和扩展滤波器算法的研究。
⽽对算法的研究多着重于将卡尔曼滤波应⽤于⾮线性系统。
为什么学界要这么热衷于将卡尔曼滤波器⽤于⾮线性系统呢？因为卡尔曼滤波器从⼀开始就是为线性系统设计的算法，不能⽤于⾮线性系统中。
但是事实上多数系统都是⾮线性的，所以如果卡尔曼滤波器不能⽤在⾮线性系统中的话，那么它的应⽤范围就⾮常有限了。
如果真的是这样，卡尔曼滤波器可能早就寿终正寝或者过很久很久才会被⼈注意到。
幸运的是早期的学者们对这个问题理解的⾮常深刻，⽽且也找到了解决⽅法，就是扩展卡尔曼滤波（EKF）。
事实上世界上的第⼀个卡尔曼滤波也是扩展卡尔曼滤波，⽽不是线性卡尔曼滤波器。
扩展卡尔曼滤波有很久远的历史，如果说有⼀个⾮线性系统需要⽤到卡尔曼滤波的话，不必怀疑，先试试扩展卡尔曼滤波准没错。
因为他有很久远的历史，所以可以轻松的找到许多这⽅⾯的资料。
不过扩展卡尔曼滤波也不是⽆懈可击的，它有⼀个很严重的短板——发散。
使⽤扩展卡尔曼滤波的时候请务必记在⼼上，时刻提醒⾃⼰，这样设计滤波器其结果会发散吗？毫不夸张地说相对于线性卡尔曼滤波设计扩展卡尔曼滤波器的就是在解决发散问题。
发散问题解决了剩下的都是⼩事。
⼩结：扩展卡尔曼滤波器主要⽤于⾮线性系统；扩展卡尔曼滤波器会发散。
线性化的卡尔曼滤波器 在讨论扩展卡尔曼滤波之前，⾸先要了解⼀下线性化卡尔曼滤波。
它和线性卡尔曼滤波器在滤波器的算法⽅⾯有同样的算法结构，⼀样⼀样的。
不⼀样的地⽅在于这两者的系统模型不同。
线性卡尔曼滤波器的系统本⾝就是线性系统，⽽线性化卡尔曼滤波器的系统本⾝是⾮线性系统，但是机智的⼤神们将⾮线性的系统进⾏了线性化，于是卡尔曼滤波就可以⽤在⾮线性系统中了。
对于⼀个卡尔曼滤波器的设计者，就不要去管你的模型到底是⼀开始就是线性系统还是⾮线性系统线性化得到的线性系统，反正只要是线性系统就好了。
卡尔曼增益推导
卡尔曼增益推导卡尔曼滤波是一种常用的估计方法，广泛应用于控制、信号处理、机器人等领域。
而卡尔曼增益则是卡尔曼滤波中的一个重要概念，它在状态估计中起到了至关重要的作用。
本文将详细介绍卡尔曼增益的推导过程。
一、背景知识在介绍卡尔曼增益之前，我们需要先了解一些背景知识。
1. 状态空间模型状态空间模型是描述系统演化的数学模型。
它由状态方程和观测方程组成。
其中，状态方程描述了系统状态如何随时间演化，而观测方程则描述了如何从系统状态中观测到数据。
2. 卡尔曼滤波卡尔曼滤波是一种递归估计方法，它通过对系统状态进行预测和更新来实现对系统状态的估计。
具体来说，它通过将当前时刻的观测值与上一时刻的估计值进行融合，得到当前时刻的最优估计值。
3. 协方差矩阵协方差矩阵描述了随机变量之间的相关性和变化幅度。
在卡尔曼滤波中，协方差矩阵用于描述状态估计的精度和可靠性。
二、卡尔曼增益的定义在卡尔曼滤波中，卡尔曼增益是用于将观测值与预测值进行融合的系数。
它的定义如下：$$K_k=P_k^-H_k^T(H_kP_k^-H_k^T+R)^{-1}$$其中，$K_k$表示第$k$时刻的卡尔曼增益；$P_k^-$表示第$k$时刻的先验估计误差协方差矩阵；$H_k$表示第$k$时刻的观测矩阵；$R$表示观测噪声的协方差矩阵。
三、卡尔曼增益推导过程接下来，我们将详细介绍卡尔曼增益的推导过程。
1. 卡尔曼滤波基本原理首先，我们需要了解一下卡尔曼滤波基本原理。
假设我们有一个线性状态空间模型：$$x_{k+1}=Ax_k+w_k$$$$y_{k}=Cx_{k}+v_{k}$$其中，$x_{k}$表示系统在第$k$时刻的状态；$y_{k}$表示系统在第$k$时刻的观测值；$w_{k}$和$v_{k}$分别表示状态噪声和观测噪声，它们都是高斯白噪声。
卡尔曼滤波的基本思想是，通过对系统状态进行预测和更新来实现对系统状态的估计。
具体来说，我们可以通过下面两个步骤来实现：（1）预测：根据上一时刻的状态$x_{k-1}$和状态转移矩阵$A$，预测当前时刻的状态$x_{k}^-$：$$x_k^-=Ax_{k-1}$$同时，我们还需要计算出先验估计误差协方差矩阵$P_k^-$：$$P_k^-=AP_{k-1}A^T+Q$$其中，$Q$表示状态噪声的协方差矩阵。
卡尔曼滤波简介及其算法实现代码
卡尔曼滤波简介及其算法实现代码卡尔曼滤波算法实现代码（C，C＋＋分别实现）卡尔曼滤波器简介近来发现有些问题很多人都很感兴趣。
所以在这里希望能尽自己能力跟大家讨论一些力所能及的算法。
现在先讨论一下卡尔曼滤波器，如果时间和能力允许，我还希望能够写写其他的算法，例如遗传算法，傅立叶变换，数字滤波，神经网络，图像处理等等。
因为这里不能写复杂的数学公式，所以也只能形象的描述。
希望如果哪位是这方面的专家，欢迎讨论更正。
卡尔曼滤波器– Kalman Filter1．什么是卡尔曼滤波器（What is the Kalman Filter?）在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。
跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。
1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。
1957年于哥伦比亚大学获得博士学位。
我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。
如果对这编论文有兴趣，可以到这里的地址下载：/~welch/media/pdf/Kalman1960.pdf。
简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。
对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。
他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。
近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。
2．卡尔曼滤波器的介绍（Introduction to the Kalman Filter）为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。
卡尔曼滤波
更新
首先要算出以下三个量：
(测量余量，measurement residual) (测量余量协方差)
(最优卡尔曼增益）
然后用它们来更新滤波器变量x与P：
https:///wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2
5/8
2017710
卡尔曼滤波 维基百科，自由的百科全书
最后两项可以抵消，得到 .
这个公式的计算比较简单，所以实际中总是使用这个公式，但是需注意这公式仅在使用最优卡尔曼增益的时 候它才成立。如果算术精度总是很低而导致数值稳定性出现问题，或者特意使用非最优卡尔曼增益，那么就 不能使用这个简化；必须使用上面导出的后验误差协方差公式。
预测
使用Jacobians矩阵更新模型
更新
预测
如同扩展卡尔曼滤波器（EKF）一样, UKF的预测过程可以独立于UKF的更新过程之外，与一个线性的（或者 确实是扩展卡尔曼滤波器的）更新过程合并来使用；或者，UKF的预测过程与更新过程在上述中地位互换亦 可。
应用
自动驾驶仪 动态定位系统 经济学，特别是宏观经济学，时间序列模型，以及计量经济学 惯性引导系统 雷达跟踪器
https:///wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2
7/8
2017710
卫星导航系统
卡尔曼滤波 维基百科，自由的百科全书
参见
即时定位与地图构建 快速卡尔曼滤波 比较：维纳滤波及the multimodal Particle filter estimator.
卡尔曼滤波器介绍
卡尔曼滤波器介绍摘要在1960年，卡尔曼出版了他最著名的论文，描述了一个对离散数据线性滤波问题的递归解决方法。
从那以后，由于数字计算的进步，卡尔曼滤波器已经成为广泛研究和应用的主题，特别在自动化或协助导航领域。
卡尔曼滤波器是一系列方程式，提供了有效的计算（递归）方法去估计过程的状态，是一种以平方误差的均值达到最小的方式。
滤波器在很多方面都很强大：它支持过去，现在，甚至将来状态的估计，而且当系统的确切性质未知时也可以做。
这篇论文的目的是对离散卡尔曼滤波器提供一个实际介绍。
这次介绍包括对基本离散卡尔曼滤波器推导的描述和一些讨论，扩展卡尔曼滤波器的描述和一些讨论和一个相对简单的（切实的）实际例子。
1 离散卡尔曼滤波器在1960年，卡尔曼出版了他最著名的论文，描述了一个对离散数据线性滤波问题的递归解决方法[Kalman60]。
从那以后，由于数字计算的进步，卡尔曼滤波器已经成为广泛研究和应用的主题，特别在自动化或协助导航领域。
第一章讲述了对卡尔曼滤波器非常“友好的”介绍[Maybeck79]，而一个完整的介绍可以在[Sorenson70]找到，也包含了一些有趣的历史叙事。
更加广泛的参考包括Gelb74；Grewal93；Maybeck79；Lewis86；Brown92；Jacobs93]. 被估计的过程卡尔曼滤波器卡用于估计离散时间控制过程的状态变量n x ∈ℜ。
这个离散时间过程由以下离散随机差分方程描述： 111k k k k x Ax bu w ---=++ （1.1）测量值m z ∈ℜ，k k k z Hx v =+ （1.2） 随机变量k w 和k v 分别表示过程和测量噪声。
他们之间假设是独立的，正态分布的高斯白噪: ()~(0)p w N Q， （1.3） ()~(0)p v N R ， （1.4）在实际系统中，过程噪声协方差矩阵Q 和观测噪声协方差矩阵R 可能会随每次迭代计算而变化。
但在这儿我们假设它们是常数。
卡尔曼滤波原理及应用matlab仿真第二版
《卡尔曼滤波原理及应用matlab仿真第二版》是一本深入探讨卡尔曼滤波原理和其在matlab中应用的专业书籍。
本书通过对卡尔曼滤波原理的详细剖析，加之丰富的matlab仿真实例，为读者提供了深入理解和应用卡尔曼滤波的宝贵资料。
本书的主要内容如下：一、卡尔曼滤波原理1. 基本概念卡尔曼滤波是一种线性最优滤波器，通过融合系统模型和实际观测值，可以对系统状态进行估计。
本书对卡尔曼滤波的基本概念进行了详细阐述，包括状态空间模型、观测模型、预测和更新等基本原理。
2. 数学推导为了帮助读者深入理解卡尔曼滤波原理，本书对卡尔曼滤波的数学推导进行了全面而系统的讲解，包括卡尔曼滤波的求解方程、卡尔曼增益的计算等内容。
3. 算法实现除了理论推导，本书还详细介绍了卡尔曼滤波算法的实现步骤，并结合matlab示例进行了实际演示，帮助读者具体了解卡尔曼滤波在实际应用中的具体操作。
二、matlab仿真应用1. matlab基础本书首先对matlab的基础知识进行了简要介绍，包括matlab的基本语法、矩阵运算、绘图函数等内容，为后续的卡尔曼滤波仿真应用做了铺垫。
2. 卡尔曼滤波仿真通过具体的matlab仿真实例，本书展示了卡尔曼滤波在不同应用场景下的具体应用，包括目标跟踪、航空航天领域、自动驾驶等领域，帮助读者从实际案例中更好地理解卡尔曼滤波的应用方法。
3. 仿真案例分析针对具体的仿真案例，本书进行了详细的分析和讨论，包括数据处理方法、滤波效果评估等内容，帮助读者深入理解卡尔曼滤波在实际应用中的具体操作步骤和注意事项。
三、实战案例与实践1. 行业案例分析本书结合实际行业案例，对卡尔曼滤波在航空航天、汽车驾驶辅助系统、无人机等领域的应用进行了案例分析，帮助读者更好地理解卡尔曼滤波在实际工程中的应用价值。
2. 实战技巧除了理论知识和仿真应用，本书还总结了在实际工程中使用卡尔曼滤波的一些实战技巧，包括滤波参数调整、模型选择、实时数据处理等方面的经验共享，为读者实际应用卡尔曼滤波提供了有益的参考。
卡尔曼(Kalman)滤波
第4章 卡尔曼（Kalman ）滤波卡尔曼滤波的思想是把动态系统表示成状态空间形式，是一种连续修正系统的线性投影算法。
功能 1） 连续修正系统的线性投影算法。
2）用于计算高斯ARMA 过程的精确有限样本预测和精确的似然函数。
3） 分解矩阵自协方差生成函数或谱密度。
4）估计系数随时间变化的向量自回归。
第一节 动态系统的状态空间表示一．假设条件令t y 表示时期t 观察到变量的一个()1n ×向量。
则t y 的动态可以用不可观测的()1r ×向量t ξ来表示，t ξ为状态向量。
t y 的动态系统可以表示为如下的状态空间模型：11t t t F v ξξ++=+ （1）t t t t y A x H w ξ′′=++ （2）其中′′F,A ,H 分别为()r r ×，()n k ×和()n r ×矩阵，t x 是外生变量或前定变量的()1k ×向量。
方程（1）称为状态方程，方程（2）称为观察方程。
其中()1r ×向量t v 和()1n ×向量t w 为向量白噪声：()()00t t Qt E v v t R t E w w t ττττττ=⎧′=⎨≠⎩=⎧′=⎨≠⎩ （3）其中,Q R 为()(),r r n n ××矩阵。
假定扰动项t v 和t w 在所有阶滞后都不相关：()0t t E v w ′= 对所有的t 和τ （4）t x 为前定或外生变量，意味着对0,1,2,....,s =除包含在121,,...,t t y y y −−之内的信息外，t x 不再能提供关于t s ξ+以及t s w +的任何信息。
即t x 可能包含y 的滞后值或所有与τ、τξ和w τ不相关变量。
状态空间系统描述有限观察值序列{}1,...,T y y ，需要知道状态向量的初始值1ξ，根据状态方程（1），t ξ可写作()123,,,...,t v v v ξ的线性函数： 2211221....t t t t t t v Fv F v F v F ξξ−−−−=+++++ 2,3,...,t T = （5）这里假定1ξ与t v 和t w 的任何实现都不相关：()()1101,2,...,01,2,...,t t E v TE w Tξτξτ′==′== （6）根据（3）和（6），得t v 和ξ的滞后值不相关：()0t E v τξ′= 1,2,...,1t t τ=−− （7） ()0t E w τξ′= 1,2,...,T τ= （8） ()()()0t t E w y E w A x H w ττττξ′′′=++= 1,2,...,1t t τ=−− （9） ()0t E v y τ′= 1,2,...,1t t τ=−− （10）二．状态空间系统的例子例1 ()AR p 过程，()()()112111...t t t p t p t y y y y µφµφµφµε+−−++−=−+−++−+ （11）()2t t E t τστεετ⎧==⎨≠⎩ （12） 可以写作状态空间形式。
卡尔曼滤波算法示例解析与公式推导
本文将对卡尔曼滤波算法进行示例解析与公式推导，帮助读者更好地理解该算法的原理和应用。
文章将从以下几个方面展开：一、卡尔曼滤波算法的概念卡尔曼滤波算法是一种用于估计动态系统状态的线性无偏最优滤波算法。
它利用系统的动态模型和观测数据，通过迭代更新状态估计值，实现对系统状态的精确估计。
卡尔曼滤波算法最初是由美国工程师鲁道夫·卡尔曼在20世纪60年代提出，随后得到了广泛的应用和研究。
二、卡尔曼滤波算法的原理1. 状态空间模型在卡尔曼滤波算法中，系统的动态模型通常用状态空间模型表示。
状态空间模型由状态方程和观测方程组成，其中状态方程描述系统的演化规律，观测方程描述观测数据与状态之间的关系。
通过状态空间模型，可以对系统的状态进行预测，并与观测数据进行融合，从而估计系统的状态。
2. 卡尔曼滤波的预测与更新卡尔曼滤波算法以预测-更新的方式进行状态估计。
在预测阶段，利用系统的动态模型和之前时刻的状态估计值，对当前时刻的状态进行预测；在更新阶段，将预测值与观测数据进行融合，得到最优的状态估计值。
通过迭代更新，可以不断优化对系统状态的估计，实现对系统状态的精确跟踪。
三、卡尔曼滤波算法的示例解析以下通过一个简单的例子，对卡尔曼滤波算法进行具体的示例解析，帮助读者更好地理解该算法的应用过程。
假设有一个匀速直线运动的物体，其位置由x和y坐标表示，观测到的位置数据带有高斯噪声。
我们希望利用卡尔曼滤波算法对该物体的位置进行估计。
1. 状态空间模型的建立我们建立物体位置的状态空间模型。
假设物体在x和y方向上的位置分别由状态变量x和y表示，动态模型可以用如下状态方程描述：x(k+1) = x(k) + vx(k) * dty(k+1) = y(k) + vy(k) * dt其中，vx和vy分别为x和y方向的速度，dt表示时间间隔。
观测方程可以用如下形式表示：z(k) = H * x(k) + w(k)其中，z(k)为观测到的位置数据，H为观测矩阵，w(k)为观测噪声。
卡尔曼滤波算法(含详细推导)PPT
3、kalman滤波算法
求式(3)所示状态向量的一步预测误差向量的相关矩阵，容易证明：
K(n1,n)E{e(n1,n)e]H(n1,n)} [F(n1,n)G (n)C (n)K ](n,n1)F [(n1,n) G (n)C (n)H ]Q 1(n)G (n)Q 2(n)G H(n)........3 ...).1 .(.
n
(n )(n 1y(1 ),y .(n .). ),
1
W 1 (k)(k)
式中W1(k)表示与一步预测项对应的权矩k 阵 1 ，且k为离散时间。
现在的问题是如何确定这个权矩阵？
（1）、状态向量的一布预测
根据正交性原理，最优预测的估计误差
e(1 nn, )x(n1)x1(n1)
12
3、kalman滤波算法
C (n )K (n ,n 1 )C H (n ) Q 2(n ).................1.).(6..
式中Q2(n)是观测噪声v2(n)的相关矩阵，而
K (n ,n 1 ) E { e (n ,n 1 )e H (n ,n 1 )}................1 ..) ....( 7 ..
这里使用了状态向量与观测噪声不相关的事实。 进一步地，由正交原理引
理知，在最小均方误差准则下求得的一步预测估 x 1 ( n )与预测误差e(n,n-1)彼
此正交，即
E{x1(n)eH(N,N1)}0
17
3、kalman滤波算法
因此，由式(26)及式(27)易得：
E {x(n1)H(n)} F(n1,n)E {x[(n)e(n,n1)e]H(n,n1)C }H(n)
卡尔曼滤波(Kalman Filter)原理与公式推导
一、背景---卡尔曼滤波的意义随着传感技术、机器人、自动驾驶以及航空航天等技术的不断发展，对控制系统的精度及稳定性的要求也越来越高。
卡尔曼滤波作为一种状态最优估计的方法，其应用也越来越普遍，如在无人机、机器人等领域均得到了广泛应用。
对于Kalman Filter的理解，用过的都知道“黄金五条”公式，且通过“预测”与“更新”两个过程来对系统的状态进行最优估计，但完整的推导过程却不一定能写出来，希望通过此文能对卡尔曼滤波的原理及状态估计算法有更一步的理解。
二、卡尔曼滤波的基本模型假设一离散线性动态系统的模型如下所示：x_{k} = A*x_{k-1} + B*u_{k} + w_{k-1}-------(1)z_{k} = H*x_{k} + v_{k} --------------------(2)其中，各变量表征的意义为：———————————————————————————x_{k}\Rightarrow 系统状态矩阵，-------， z_{k}\Rightarrow 状态阵的观测量（实测）A\Rightarrow 状态转移矩阵，-------， B\Rightarrow 控制输入矩阵H\Rightarrow 状态观测矩阵w_{k-1}\Rightarrow 过程噪声，-------，v_{k}\Rightarrow 测量噪声———————————————————————————如果大家学过《现代控制理论》的话，对上述模型的描述形式一定不会陌生，只是多了变量 w_{k-1} 与 v_{k} 。
其中，随机变量w_{k-1} 代表过程噪声（process noise）， v_{k} 代表测量噪声（measurement noise），且为高斯白噪声，协方差分别为 Q 和 R ，即 p(w) \in N(0,Q) ， p(v) \in N(0,R) 。
为什么要引入这两个变量呢？对于大多数实际的控制系统（如倒立摆系统）而言，它并不是一个严格的线性时变系统(Linear Time System)，亦或系统结构参数的不确定性，导致估计的状态值x_{k} 存在偏差，而这个偏差值由过程噪声 w_{k} 来表征。
卡尔曼滤波器原理详解
在卡尔曼增益公式两侧同时右乘
得：
SkKk
T
KkSkKk
T
T T ˆ PkH Kk
把上式带入3.5式，可以消去后面的两项，得：
ˆk P ˆ k KkHP ˆk P
卡尔曼滤波
3
算法推导
整理，得：
ˆ k ( I KkH ) P ˆk P
这个公式的计算比较简单，所以实际中总是使用这个 公式，但是需注意这公式仅在使用最优卡尔曼增益的 时候它才成立。如果算术精度总是很低而导致数值稳 定性出现问题，或者特意使用非最优卡尔曼增益，那 么就不能使用这个简化；必须使用3.5式表示的后验误 差协方差公式。
2、根据先验估计
这里的权重系数也成为卡尔曼增益。 至此卡尔曼滤波计算原型公式基本结束， 下面要确定出最优系数K和在最优系数下 的后验估计误差的协方差矩阵的递推方 法 权重 残差
卡尔曼滤波
3
算法推导
ˆk 推导后验协方差矩阵 P ˆ k 开始推导如下： 按照定义，我们从误差协方差 P
ˆ k cov(xk x ˆk ) P
nx1 真值
2.5 2.6
卡尔曼滤波
2
算法前提
定义先验估计误差的协方差为
Pk E{ek , ek } cov( ek )
T
2.7
定义 后验估计误差的协方差为
Pk E{ek , ek } cov( ek )
T
2.8
卡尔曼滤波
3
算法推导 目的描述：
在系统结构已知的情况下，给定k时刻的状态观测向 量 k，求k时刻的系统状态向量的最优估计 k ，使 得 Pk 最小。
z
ˆ x
基本思路：
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
卡尔曼滤波-- 从推导到应用(一)2013-12-30 01:26 1328人阅读评论(9) 收藏举报滤波算法该文是自我总结性文章，有纰漏，请指出，谢谢。
--白巧克力前言卡尔曼滤波器是在估计线性系统状态的过程中，以最小均方差为目的而推导出的几个递推数学等式,也可以从贝叶斯推断的角度来推导。
本文将分为两部分：第一部分，结合例子，从最小均方差的角度，直观地介绍卡尔曼滤波的原理，并给出较为详细的数学推导。
第二部分，通过两个例子给出卡尔曼滤波的实际应用。
其中将详细介绍一个匀加速模型，并直观的对比系统状态模型的建立对滤波的影响。
第一部分先看一个对理解卡尔曼滤波能起到作用的的笑话:一片绿油油的草地上有一条曲折的小径,通向一棵大树.一个要求被提出:从起点沿着小径走到树下.“很简单.” A说,于是他丝毫不差地沿着小径走到了树下.现在，难度被增加了：蒙上眼。
“也不难，我当过特种兵。
” B说，于是他歪歪扭扭地走到了树旁。
“唉，好久不练，生疏了。
” （只凭自己的预测能力）“看我的，我有DIY 的GPS！” C说，于是他像个醉汉似地歪歪扭扭的走到了树旁。
“唉，这个GPS 没做好，漂移太大。
”（只依靠外界有很大噪声的测量）“我来试试。
” 旁边一也当过特种兵的拿过GPS, 蒙上眼，居然沿着小径很顺滑的走到了树下。
（自己能预测+测量结果的反馈）“这么厉害！你是什么人?”“卡尔曼! ”“卡尔曼？！你就是卡尔曼？”众人大吃一惊。
“我是说这个GPS 卡而慢。
”此段引用自 highgear 的《授之以渔：卡尔曼滤波器...大泄蜜...》 (点击可跳转到该网页)这个小笑话很有意思的指出了卡尔曼滤波的核心，预测+测量反馈，记住这种思想。
-----------------------------------------------------------分割线-----------------------------------------------------------------------在介绍卡尔曼滤波前，简单说明几个在学卡尔曼过程中要用到的概念。
即什么是协方差，它有什么含义，以及什么叫最小均方差估计，什么是多元高斯分布。
如果对这些有了了解，可以跳过，直接到下面的分割线。
均方差：它是"误差"的平方的期望值（误差就是每个估计值与真实值的差），也就是多个样本的时候，均方差等于每个样本的误差平方再乘以该样本出现的概率的和。
（均方差即为标准差，为方差的开根号？）方差：方差是描述随机变量的离散程度，是变量离期望值的距离。
注意两者概念上稍有差别，当你的样本期望值就是真实值时，两者又完全相同。
最小均方差估计就是指估计参数时要使得估计出来的模型和真实值之间的误差平方期望值最小。
两个实变量之间的协方差：它表示的两个变量之间的总体误差，当Y=X的时候就是方差。
下面说说我对协方差的通俗理解，先抛去公式中的期望不谈，即假设样本X,Y发生的概率就是1，那么协方差的公式就变成了：这就是两个东西相乘，马上联想到数值图像里的相关计算。
如果两个变量的变化趋势一致，也就是说如果其中一个大于自身的期望值，另外一个也大于自身的期望值，那么两个变量之间的协方差就是正值。
如果两个变量的变化趋势相反，即其中一个大于自身的期望值，另外一个却小于自身的期望值，那么两个变量之间的协方差就是负值。
协方差矩阵只不过就是元素多了组成了矩阵，其中协方差矩阵的对角线就是方差，具体公式形式请见wiki。
其实，这种相乘的形式也有点类似于向量投影，即两个向量的内积。
再远一点，联想到傅里叶变换里频谱系数的确定，要确定一个函数f(x)在某个频率w上的频谱，就是<f(x),cos(wt)>,< ,>表示向量内积，通俗的讲是将f(x)投影到cos(wt)上，要讲清傅里叶的本质需要另写一篇博文，这里提到这些只是觉得有益于对知识的相互理解。
高斯分布：概率密度函数图像如下图,四条曲线的方差各不相同,方差决定了曲线的胖瘦高矮。
（图片来源：维基百科)多元高斯分布：就是高斯分布的低维向高维的扩展，图像如下。
对应多元高斯分布的公式也请自行谷歌，以前高斯公式中的方差也变成了协方差，对应上面三张图的协方差矩阵分别如下：注意协方差矩阵的主对角线就是方差，反对角线上的就是两个变量间的协方差。
就上面的二元高斯分布而言，协方差越大，图像越扁，也就是说两个维度之间越有联系。
-----------------------------------------------------------分割线---------------------------------------------------------------------这部分每讲一个数学性的东西，接着就会有相应的例子和直观的分析帮助理解。
首先假设我们知道一个线性系统的状态差分方程为其中x是系统的状态向量，大小为n*1列。
A为转换矩阵，大小为n*n。
u为系统输入，大小为k*1。
B是将输入转换为状态的矩阵，大小为n*k。
随机变量w为系统噪声。
注意这些矩阵的大小，它们与你实际编程密切相关。
看一个具体的匀加速运动的实例。
有一个匀加速运动的小车，它受到的合力为ft , 由匀加速运动的位移和速度公式，能得到由t-1 到t 时刻的位移和速度变化公式：该系统系统的状态向量包括位移和速度，分别用xt 和xt的导数表示。
控制输入变量为u，也就是加速度，于是有如下形式:所以这个系统的状态的方程为：这里对应的的矩阵A大小为2*2 ,矩阵B大小为2*1。
貌似有了这个模型就能完全估计系统状态了，速度能计算出，位移也能计算出。
那还要卡尔曼干嘛，问题是很多实际系统复杂到根本就建不了模。
并且，即使你建立了较为准确的模型，只要你在某一步有误差，由递推公式，很可能不断将你的误差放大A倍（A就是那个状态转换矩阵），以至于最后得到的估计结果完全不能用了。
回到最开始的那个笑话，如果那个完全凭预测的特种兵在某一步偏离了正确的路径，当他站在错误的路径上（而他自己以为是正确的）做下一步预测时，肯定走的路径也会错了，到最后越走越偏。
既然如此，我们就引进反馈。
从概率论贝叶斯模型的观点来看前面预测的结果就是先验，测量出的结果就是后验。
测量值当然是由系统状态变量映射出来的，方程形式如下：注意Z是测量值，大小为m*1(不是n*1，也不是1*1，后面将说明），H也是状态变量到测量的转换矩阵。
大小为m*n。
随机变量v是测量噪声。
同时对于匀加速模型，假设下车是匀加速远离我们，我们站在原点用超声波仪器测量小车离我们的距离。
也就是测量值直接等于位移。
可能又会问，为什么不直接用测量值呢？测量值噪声太大了，根本不能直接用它来进行计算。
试想一个本来是朝着一个方向做匀加速运动的小车，你测出来的位移确是前后移动（噪声影响），只根据测量的结果，你就以为车子一会往前开一会往后开。
对于状态方程中的系统噪声w和测量噪声v，假设服从如下多元高斯分布，并且w,v是相互独立的。
其中Q,R为噪声变量的协方差矩阵。
看到这里自然要提个问题，为什么噪声模型就得服从高斯分布呢？请继续往下看。
对于小车匀加速运动的的模型，假设系统的噪声向量只存在速度分量上，且速度噪声的方差是一个常量0.01，位移分量上的系统噪声为0。
测量值只有位移，它的协方差矩阵大小是1*1，就是测量噪声的方差本身。
那么：Q中，叠加在速度上系统噪声方差为0.01，位移上的为0，它们间协方差为0，即噪声间没有关联。
理论预测（先验）有了，测量值（后验）也有了，那怎么根据这两者得到最优的估计值呢？首先想到的就是加权，或者称之为反馈。
我们认定是预测（先验）值，是估计值，为测量值的预测，在下面的推导中，请注意估计和预测两者的区别，不混为一谈。
由一般的反馈思想我们得到估计值：其中，称之为残差，也就是预测的和你实际测量值之间的差距。
如果这项等于0，说明预测和测量出的完全吻合。
这种反馈递推的形式又让我联想到数值分析里用来求解线性方程组时的一种迭代方法，Gauss-Seidel迭代法，有兴趣的可以看看。
现在的关键就是求取这个K。
这时最小均方差就起到了作用，顺便在这里回答为什么噪声必须服从高斯分布，在进行参数估计的时候，估计的一种标准叫最大似然估计，它的核心思想就是你手里的这些相互间独立的样本既然出现了，那就说明这些样本概率的乘积应该最大（概率大才出现嘛）。
如果样本服从概率高斯分布，对他们的概率乘积取对数ln后，你会发现函数形式将会变成一个常数加上样本最小均方差的形式。
因此，看似直观上很容易理解的最小均方差理论上来源就出于那里（详细过程还请自行谷歌，请原谅，什么都讲的话就显得这边文章没有主次了）。
先看估计值和真实值间误差的协方差矩阵，提醒一下协方差矩阵的对角线元素就是方差，求这个协方差矩阵，就是为了利用他的对角线元素的和计算得到均方差.这里请注意ek是向量，它由各个系统状态变量的误差组成。
如匀加速运动模型里，ek便是由位移误差和速度误差，他们组成的协方差矩阵。
表示如下：其中，Serr代表位移误差，Verr代表速度误差,对角线上就是各自的方差。
把前面得到的估计值代入这里能够化简得：(1)式同理，能够得到预测值和真实值之间误差的协方差矩阵：注意到系统状态x变量和测量噪声之间是相互独立的。
于是展开（1）式可得：最后得到：继续展开：接下来最小均方差开始正式登场了，回忆之前提到的，协方差矩阵的对角线元素就是方差。
这样一来，把矩阵P的对角线元素求和，用字母T来表示这种算子，他的学名叫矩阵的迹。
最小均方差就是使得上式最小，对未知量K求导,令导函数等于0，就能找到K的值。
注意这个计算式K，转换矩阵H式常数，测量噪声协方差R也是常数。
因此K的大小将与预测值的误差协方差有关。
不妨进一步假设，上面式子中的矩阵维数都是1*1大小的，并假设H=1，不等于0。
那么K可以写成如下：所以越大，那么K就越大，权重将更加重视反馈,如果等于0，也就是预测值和真实值相等，那么K=0，估计值就等于预测值（先验）。
将计算出的这个K反代入Pk中，就能简化Pk，估计协方差矩阵Pk的：因此递推公式中每一步的K就计算出来了，同时每一步的估计协方差也能计算出来。
但K的公式中好像又多了一个我们还未曾计算出来的东西,他称之为预测值和真实值之间误差的协方差矩阵。
它的递推计算如下：请先注意到预测值的递推形式是：由于系统状态变量和噪声之间是独立，故可以写成：由此也得到了的递推公式。
因此我们只需设定最初的,就能不断递推下去。
这里总结下递推的过程，理一下思路：首先要计算预测值、预测值和真实值之间误差协方差矩阵。
有了这两个就能计算卡尔曼增益K，再然后得到估计值，最后还要计算估计值和真实值之间的误差协方差矩阵，为下次递推做准备。