卡尔曼滤波器笔记——最详细

小橙不吃辣椒

已于 2024-03-17 11:48:51 修改

阅读量732

点赞数 26

文章标签：笔记深度学习算法卡尔曼滤波

于 2024-03-13 00:41:22 首次发布

本文链接：https://blog.csdn.net/Jochen12345/article/details/136608994

版权

笔记来源—

卡尔曼滤波算法原理及代码实现！https://www.bilibili.com/video/BV1WZ4y1F7VN/?spm_id_from=333.337.search-card.all.click&vd_source=8d55784dc9c7530bc9e3fa220380be56

简单介绍一下

现在我们就是不知道是距离多少，就需要用到这个卡尔曼滤波器。

这里的预测方程就是我们的状态方程

这里的 $H$ 一般就是单位矩阵，或者是单位矩阵的一部分

这里的 $n$ 一般就是状态变量的个数, $m$ 就是你选择的观测值

$H$ 具体是什么形状要根据选择的状态变量 $X_{k}$ 以及要观测的目标值的 $Z_{k}$ 的形状

所以上面我们举的小车的例子的观测值就只有一个，那就是他的距离，所以 $Z_{k}$ 就是1*1的矩阵， $m$ 就是1，如果我们还想知道车速，那么 $m$ 就是2. $Z_{k}$ 就是2*1的矩阵

举个例子

状态变量的个数等于状态方程的个数乘以他的阶数，这里他只有一个方程

首先要看他是一个几自由度的模型，这个RLC模型他是一个1自由度的系统，所以他只涉及一个方程，因为他只有一个回路，一个方程就可以把这个系统表示了。本质上他是一个单自由度系统。一个单自由度系统就写一个方程，这个方程他又是一个二阶的方程，所以选两个状态变量。

这里我们的求导是为了些状态方程。

这里的 $A$ 是系统矩阵， $B$ 是控制矩阵

$X_{k-1}$ 代表的就是在k-1时候的状态， $u_{k-1}$ 就是我们的控制量，一般我们在基尔霍夫数据中里面一般是认为没有控制量，所以这一项基本上为0，再加上我们的噪声 $w_{k-1}$ 就得到了预测方程

问题来了，那么我们的观测方程又是什么？

H到底是什么？

H其实就是一个单位阵，继续用那个例子

我们的 $X_{k}=\begin{bmatrix} I\\ \frac{1}{C}\int Idt\end{bmatrix}$

$Z_{k}=HX_{k}+V_{k}$ 是由我们自己决定的，也就是说你做卡尔曼滤波就是你决定的，①比如是电流 $I$ 的话我们的 $Z_{k}$ = $I$ .②比如还想要 $\frac{1}{C}dt$ ，那么 $Z_{k}=$ $\begin{bmatrix} I\\ \frac{1}{C}\int Idt\end{bmatrix}$ 。

在第一种情况下我们的 $H=\begin{bmatrix} 1 & 0 \end{bmatrix}$

也就是说 $Z_{k}=HX_{k}+V_{k}$ ,我们只要知道 $Z_{k}$ 跟 $X_{k}$ 就可以知道我们的 $H$

如果我们选第二种，那么我们的 $H=\begin{bmatrix} 1 &0 \\ 0& 1 \end{bmatrix}$ ,但是他应该是 $H=\begin{bmatrix} 1 &0 \\ 0& 0 \end{bmatrix}$ ，因为什么呢,因为

$\frac{1}{C}\int Idt$ 无法观测到，写了1就说明观测的到，但是我们观测不到所以这里是0.

我们的目标就是找到最优的估计值，先验就是我们要通过我们的预测方程来达到一个预测估计值

$\hat{x}_{k}$ 就是说我们已经测量 $z_{k}$ 了，也就是说已经拿到预测值 $\hat{x}_{k}^{-}$ 还有测量值 $z_{k}$ 了,我们对他进行矫正，矫正得到了一个最优的估计值，然后就会得到我们的先验估计误差和后验估计误差

下面就是我们的预测方程，上面讲过

然后我们找到了先验估计误差跟后验估计误差，就有了先验估计误差的协方差还有后验估计误差的协方差

优化思路

卡尔曼滤波详细解释

我们对一个硬币进行测量三次，那我们怎么去估计这个值呢，一般我们的第一思路就是求平均值

经过转换我们可以看到这个等式

随着k的增加我们可以知道，这个 $\frac{1}{k}$ 趋近于0

这个也很好理解，就是说当我们有了大量的数据，我们对估计的结果就很有信心了

数据比较少的时候，作用就大了

让我们换种表达方式

可以看到我们的新一次的估计值与上一次的估计值有关，上一次的就跟上上一次的有关，这就是卡尔曼滤波的递归思想。

这个时候我们引入估计误差跟测量误差的概念，我们可以知道当估计误差远大于测量误差的时候，我们的卡尔曼增益是趋近于1的，这个时候我们就带入原来的公式，可以看到我们的估计值是等于测量值的，也就是说我们更加相信这个测量值。

当估计误差远小于测量误差的时候，我们的卡尔曼增益系数就接近于0，我们的估计值就相信上一次的估计值

举个例子

我们先引入三个公式，前面两个就是我们上面推出来的公式，第三个是我们的更新估计误差的公式

再引入我们的数据，还是测长度。

我们的实际长度是50mm，估计长度是40mm，估计误差在5mm，我们第一次的测量值为51mm，测量误差是3mm，也就是说我们测量的值在48-54mm之间，使用的是同一个测量工具，所以它第几次的测量误差都是3mm

然后我们就可以进行计算

然后我们继续进行第二次的计算

我们第二次测量的值为48，测量误差还是3mm

我们继续进行计算，可以看到我们的卡尔曼增益根据上一次的估计误差进行了更新

我们的估计值也由于卡尔曼的增益和上一次的估计值进行了更新

我们的估计误差根据我们的卡尔曼增益跟上一次的估计误差进行了更新

然后剩下的我们使用excel进行计算

我们的测量误差就是3，只要保证在50上下范围3左右浮动就可以了

然后先计算我们的 $K_{k}$

再计算我们的估计值 $\hat{x}_{k}$

再计算我们的估计误差

这里可以看到这里的蓝色是我们的测量结果，红色是我们估计的结果，我们的估计值从一开始的40开始上升，慢慢逐渐到我们的49附近，再慢慢靠近我们的实际值，我们知道这个50是我们的实际尺寸

数学基础——数据融合，协方差矩阵，状态空间方程，观测器

方差

这两个是一样的

数据融合——Data Fusion

假设我们现在有两个称，去称一个东西，得到两个结果，而且我们知道这两个称都不准，得到两个结果一个是 $Z_{1}$ ，另一个是 $Z_{2}$

我们可以知道他的正态分布和概率（积分可以算）

机器视觉中应用正态分布

正态分布概率计算

不知道正态分布的的可以看一下上面这两个链接

这时候我们知道了这两个结果，这个时候我们要去估算这两个真实值，是多少呢？

凭感觉来讲，应该是在这两个分布的中间，而且第一个的误差小，因为他的标准差小

所以真实值更靠近第一个称称出来的结果，大概在绿线的位置

如果要从数学上找到一个最优的估计值应该怎么做？

这个时候就要用到我们之前所说的Kalman Gain的思想了

下面我们就是要去求解K了，就是使得我们的估计值的标准差最小，也就是方差最小

为什么？

因为方差越小越接近真实值

对其求导（令他为0），可以得到我们的极值，然后再带入我们的方差值，就可以知道我们的k了

有了k以后我们再把k带入上面的式子

可以得到

也就是说，根据这两个称的特性测量出来的结果我们做出了预测，这个预测就是30.4g，并且通过数学证明了他就是最优解

回到我们之前的这一步，我们，这时候我们就可以去更新我们的估计误差

这个时候去计算他的方差

带入到蓝色的地方

就得到了我们估计值跟估计误差（方差）

这就是他的图，这个过程就叫做数据融合

协方差矩阵

举个例子

这里我们取三个人最基本的数据，身高，体重，年龄。并求平均值，方差，协方差

从这里的协方差可以反应出如果这里两个相乘项都小于平均值，那么两个数相乘都是正的，如果都大于平均值，两个数相乘也是正的。如果一个大一个小，那么两个值相乘就是负的了。

结果：1、如果求出来的协方差两个值都是正的话，那就说明这两个变量方向就是一样的（正相关）

2、如果是负的话，那就说明这两个变量变化方向是相反的

同样我们可以求出来另外两个的协方差

然后我们的协方差矩阵就是这种形式，带入数据，我们就可以分析各个数据之间的关系了

然后我们再可以扩展一下，在扩展之前，我们分析一下

如何通过矩阵的运算来计算协方差

首先我们去求一个过渡矩阵

我们可以知道a矩阵的后面两个矩阵相乘再乘三分之一就是一个求平均值

计算一下，其实就是对应的，通过这样的方法我们就能算出来协方差矩阵了

这里我们给出了15个队员，然后右边的就是他们的协方差矩阵，我们先看对角线的数据

可以看到他们之间的方差变化是蛮大的

也就是说如果想成为射手的话，并不局限于身高和体重，年龄的跨度也是比较大的，19.4的方差相当于有四岁多的标准差，±4岁也就是8岁，跨度也是不少的，所以说一个射手的年龄关系不是特别大。

首先我们看体重跟身高，首先就是29.75，说明体重跟身高是非常正相关的，随着身高增加体重也增加，然后我们看年龄跟体重，年龄跟身高，他们之间的相关性非常小。说明对于这些运动员来说

他们的身高体重年龄就没有太多的关系。

状态空间表达

现代控制理论就是以状态空间为基础的，有兴趣可以看这个up主的视频

【Advanced控制理论】1_介绍_哔哩哔哩_bilibili

这里我们给出最基础的概念

例子

一个弹簧阻尼系统，质量为m,向右施加的力为F，向右的方向是x，弹簧的胡克系数是k，B是他的阻尼系数。他的动态方程如下图所示

然后我们可以把F定义为u，也就是系统的输入。

首先我们把他化成状态空间的表达式就是首先要确定两个状态变量。

然后做一个等量代换，就用两个一阶微分方程把这个形式表达出来了。

如果我们这里还有测量量，我们可以把它定义为Z1，测的是他的位置，Z2测的就是它的速度

然后我们把上面的4个式子用矩阵的方式紧凑的表达出来

我们就去填一个数

根据上面的式子可以得到

而测量的方程就可以这样表达

完整的表达

这就是状态空间的表达形式

我们可以归纳一下（蓝字）

这就是一种连续的表达形式

我们可以看到状态变量的方程哪里有对Xt的导数，体现了随时间的变化

如果我们将其写成离散的形式，每相隔一个时刻来看一下这个变化的话，就可以写成这样的形式

这里的下标k,k-1,k+1每一个单位1都代表了一个时间单位，叫做sample time（采样时间）也代表了一种变化趋势

从离散型到连续型并不是照抄上面的矩阵结果，需要根据采样时间来进行计算（不是这次重点讨论的结果，只需要记得基本的概念）,体现的是一种变化，从上一步到这一步的变化。

然后我们回到之前的讲过的

数据融合的例子

我们知道世界中，充满了不确定性，比如我们的模型不准确，这时候我们就可以加一点不确定性，加一个 $w_{k-1}$ ,在测量当中我们也可以加上 $v_{k}$ ，这是在测量当中产生的不确定性。现在我们就是有了两个不确定性，也就是说模型也不准确，测出来的值也不准确。

在这两个都不准确的情况下，如何去测量一个精确的 $\hat{x}_{k}$ 呢？

这就是我们卡尔曼滤波器去解决的问题

我们现在遇到的情况跟之前的也是差不多的，只不过我们现在是一个不太准的计算结果和一个不太准的测量结果，我们要根据这两个结果来估计出来一个相对准确的值。来找到一个比他们两个误差都要小的结果，这也就是我们后面要重点分析的。

卡尔曼滤波详细数学推导

引入概念

期望是什么？

在统计学和概率论中，"期望"是一个随机变量的平均值，表示该随机变量的平均预期结果。在离散随机变量的情况下，期望可以通过对所有可能取值的概率加权平均得到。在连续随机变量的情况下，期望则是对变量的值乘以其概率密度函数（PDF）进行积分得到。

设随机变量为X，它的取值为{x1, x2, ..., xn}，对应的概率为{p1, p2, ..., pn}，则X的期望E(X)可以表示为：

E(X) = x1p1 + x2p2 + ... + xn*pn

换句话说，期望就是所有可能取值的加权平均值。

例如，假设有一个掷骰子的游戏，骰子的点数为1到6，每个点数出现的概率都是1/6。那么掷骰子的期望就是：

E(X) = (11/6) + (21/6) + (31/6) + (41/6) + (51/6) + (61/6) = 3.5

这表示掷骰子的平均预期点数为3.5。

方差是什么？

方差是衡量随机变量离散程度的一个统计量，它表示随机变量与其期望值之间的偏离程度。如果一组数据的方差较大，则说明数据的离散程度较高；反之，方差较小则说明数据较为集中。

可以跟着一起拿纸算一下，在之前我们了解了状态空间方程的表达形式，借此来描述一个系统的动态响应

这里面我们 $x_{k}$ 是他的状态变量， $A$ 是他的状态矩阵， $B$ 是控制矩阵， $u_{k-1}$ 是控制， $w_{k-1}$ 是过程噪声，然后过程噪声是我们不可测的，也是我们没有办法掌握的一个内容，因为他是一个不确定性的表现，在自然界我们一般将其假设为符合正态分布，也就是他的概率分布。

他的概率分布就是也就是概率分布 $p(w)$ ,0代表了它的期望，Q代表了他的协方差矩阵

然后Q又等于w乘w的转置

这个公式是怎么来的呢？我们用简单的例子说明一下

假如我们现在有两项，假设 $x_{k}$ 有x1,x2两项。这边他的过程噪声也自然就是w1,w2了

协方差Cov(X,Y)=E(XY)-E(X)E(Y),因为E(w1)和E(w2)都为0，所以E(w1w2)=Cov(w1,w2))

然后求出来的就是我们的协方差矩阵，对角线分别代表了w1的协方差跟w2的协方差，另外两个代表w1,w2的协方差。这个不仅能表达w1,w2的协方差，还可以表现出w1,w2之间的关系

实际上应该类似这样表示 $Cov(w_{1},w_{1})$ , $Cov(w_{2},w_{2})$ ,

当然另外也有我们的过程噪声v，计算方法也是跟我们上面w是一样的

此外， $w_{k-1}$ 跟 $v_{k}$ 这一项是没有办法去建模的，所以我们唯一可以掌握的就是前面的部分，

由于我们只知道前面两个的值，而不知道后面的噪声，我们就只能知道这个估计值，就被我们称之为先验估计 $\hat{x}_{\bar{k}}$

$x_{k-1}$ 也是上一次状态的估计值，也是我们所得不到的，只能根据上一次的估计结果来判断

然后我们再来看 $z_{k}$

通过我们的 $z_{k}$ 可以得出

$\hat{x}_{k mea}$ 应该是通过测量得出的，对xk的估计

然后我们现在就有了两个结果，一个是先验的结果（算出来的），还有一个就是测出来的结果

然后我们发现不论是算出来的还是测出来的，都不具备噪声的影响，所以是不准确的

然后我们这里就可以用到我们之前提到过的数据融合的方法

这里面我们就可以用同样的方法

然后我们就知道，我们的后验就是这个公式，当我们的G=0的时候，我们更加相信先验的结果，后验就更加相信测量的结果

然后我们知道教科书给出的是下面这种形式，但是我们把我们的G带入就可以发现实际上是这样

然后这里我们的 $K_{k}$ 的取值范围就是0到H逆，取0的时候就更加相信算出来的结果，而 $K_{k}$ 为H逆的时候就更加相信算出来的结果，而我们的目标就很清晰了，我们要使得我们的误差最小，也就是我们的估计值 $\hat{x_{k}}$ 更趋近于我们的实际值 $x_{k}$

如何去选择我们的 $K_{k}$ ？

那这就跟我们的噪音有关系

比如说我们测量误差大就相信计算的结果，而我们计算的误差大我们就更相信测量的结果

所以 $K_{k}$ 的取值跟误差息息相关

我们引入误差

然后我们的误差也是符合正态分布

然后他的协方差矩阵我们用P来表示，也是符合之前我们说的

距离实际值越小也就是说我们整个误差的方差最小

方差最小也就是说越接近我们的期望值，越接近于0

所以我们希望选取合适的K，使得P矩阵的tr(p)最小，也就是迹最小，也就是我们的对角线相加最小，自然方差就是最小

下面我们就是要来计算它的协方差矩阵了

我们希望状态变量的估计值能尽可能接近真实值，否则搞那么多就没意义了。因此引入e_k这个误差，期望是\hat{x}_k等于真实值，所以它们的误差为0即误差的期望为0，而方差越小状态变量越接近真实值

$e_{k}$ 可能是多维的。

我们带入一下

带入原来的公式

根据转置的性质我们可以知道

求这个整体的期望就相当于求每一项的期望，因为他们都是线性的，

我们可以看到这两个都是常数

也就是说期望是看他的期望

在这里 $e_{k\bar{}}$ 是先验误差， $v_{k}$ 是测量误差，这两个是没有关系的

所以他们两个相互独立，而我们又知道这两个都是0，所以

同理我们另外一项也是0

然后我们再跟这个下面这个做对比

这里就是一个期望了， $E(e_{k}^{-}e_{k}^{-T})$ = $P_{k^{-}}$ 这个就是先验误差的协方差矩阵，然后我们继续计算

这里的 $I$ 是我们的单位矩阵，然后我们继续看回到我们的P（v）, $E(VV^{T})$ 就是 $R$

再回到我们原来的公式

带入就知道，然后我们继续展开

我们就知道这就等于误差的协方差矩阵

回到我们的目标就是让他的迹最小

这个时候我们就要求他的迹

这也就是 $P_{k}$ ,也就是误差的协方差矩阵的迹，然后这个时候我们要寻找K，使得他有最小值

也就是我们这里对 $K_{k}$ 求导，也就是我们这里等于0，就可以找到极值点了

我们开始计算

继续计算

协方差矩阵转置的矩阵等于他本身就是这里的 $P_{k}^{-}$

后面我们算出来下面这个就是卡尔曼滤波最核心的公式

其实我们对其进行分析也可以看出来，当我们的 $R$ 特别大的时候

$R$ 就是我们测量噪声矩阵的协方差， $R$ 特别大就说明噪声的方差特别大，其特别大的时候，整个式子 $K_{k}$ 就趋近于0了，而 $K_{k}$ 趋近于0的时候， $\hat{X}_{k}$ 就等于先验估计，因为测量R是我们的测量噪声，也就是我们更愿意相信先验的结果，也就是相信我们计算的结果。

如果我们的 $R$ 趋近于0的话，也就是我们测量误差非常非常小

我们这里就是H的逆了

其实也很好理解，如下图

卡尔曼滤波器的五个公式

这里面我们绿色的都是已知的，红色就是我们未知的要我们求的，我们这次的目的就是求 $P_{k\bar{}}$

然后根据 $P_{k\bar{}}$ 的定义，我们带入得出他的方程式，并且带入到原来的公式

然后我们发现这里有一个相互独立，这是为什么呢？

上一次的误差等于上一次的真实值减去上一次的估计值

而我们的 $w_{k-1}$ 作用在 $x_{k}$ 上，是作用在这一次上，而 $e_{k-1}$ 是上一次上，所以这两个相互独立

所以中间两个我们的期望就都为0

这里我们第一项就可以把我们的常数取值拿出来，就变成了这样，第一项我们的期望就是P的k-1次的误差的协方差矩阵，然后后面的这一项就是我们过程误差的协方差矩阵，就等于Q

最后结果（先验误差的协方差矩阵）就变成我们蓝框里的值了

有了这个式子，我们就可以用卡尔曼滤波器来估计状态变量的值了

分为两个步骤，一个是预测，一个是矫正

显示进行先验估计，然后我们再更新误差协方差矩阵，再使用我们的卡尔曼增益，最后得到我们的后验估计，在进行更新误差协方差

这里他的每一次预测都会用到上一次的结果，一次一次的往回推，直到我们推到了第0次，也就是最开始的时候，我们需要给他赋予一个初值分别是 $\hat{x}_{0}$ 跟 $p_{0}$

卡尔曼滤波实例

我们可以看到这里有一个人在匀速直线运动，下图是其方程，为了方便我们把采样时间间隔设置为1

可以看出来这两个方程是在理想状态下的，但是我们现实生活中是有误差的，比如上坡跟下坡，而且我们走的过程中也会有时候快有时候慢，所以我们这个时候的数学模型就要加上不确定性

所以我们加上了过程噪声，且假设他们的这个噪声是符合自然界的正态分布的，期望为0，协方差矩阵是Q

除了这个数学模型，我们还可以在天上放一个卫星来时刻监测这个人的位置跟距离，这样的话就得到了测量方程，测量两个内容，如下图

如果说这两个测量准确的话，那就可以精确的知道在k时刻时，那个人的位置跟速度了，也就用不上滤波器了，但是在现实生活中都是存在不确定性的，可能会测到不同物体，而且我们的传感器也会有误差，所以我们也加上噪声，并且他也符合正态分布

然后我们将其归纳一下再表达出来

这样一来我们再去估计 $X_{1,k}$ 跟 $X_{2,k}$ 的值的话，想得到一个相对准确的结果的话，这就是一个数据融合的概念。

通过一个不太准确的数学模型跟一个不太准确的测量结果去估计一个最优值，而这个估计最优值的算法就是卡尔曼滤波器，说他是最优算法，是因为他在数学上得到了证明

而我们的卡尔曼滤波器分为两个步骤，上面我们也讲过也可以直接去看视频

下载好对应的文件

【卡尔曼滤波器】5_直观理解与二维实例【包含完整的EXCEL代码】_哔哩哔哩_bilibili

这里面开头介绍了如何下载

比如这里我们有两个误差是随机数，是用语句产生的

我们看到这里的标准差，这里是我们这里的开平方，Q矩阵跟R矩阵是只有对角线上是有数的

然而旁边的两项是0，也就是说我们只有方差这两项而没有协方差这两项

这个文件里蓝色部分我们是可以去修改的

把他的两个分别都改一下，再看一下我们的

我们可以看到黄色的线跟橘红色的线非常靠近，因为我们测量的协方差要小，所以我们后验更加靠近测量的结果

如果我们将估计的误差变小，测量的误差变大

这个时候我们的测量值，也就是这个橘红线离蓝线非常远，因为他的测量误差很大，但是经过我们的卡尔曼滤波器以后，我们发现后验的估计速度并没有因为很大的测量偏差而走的非常远，反而是由于我们有这个先验的计算结果，会把后验的距离拉的与真实值非常近

我们可以通过修改这里面的数值来学习卡尔曼滤波器

扩展卡尔曼滤波器

之前我们知道我们的卡尔曼滤波器的五个公式

也是我们数据融合的思想

这是我们的线性系统，但是对于我们的非线性系统，他无法用这种方式来表达，而是下面这种形式

但无论是线性还是非线性，他们的误差都是可以用正态分布来表达的

但这里有个问题

我们就需要用到线性化，我们就需要用到泰勒级数

用一个一维的例子来看就是，我去预测一个x就需要找到一个 $x_{0}$

这里就是一个简单的三角的几何关系

然而对于高纬度我们就需要用到雅可比矩阵，关于这部分内容我们可以参考

【工程数学基础】2_线性化_泰勒级数_泰勒公式_Linearization_哔哩哔哩_bilibili

可以看的出来我们想去线性化一个系统我们需要去找到一个点(Operating Point)，对他附近进行线性化

对于我们非线性系统来说，最好的线性化的点就是真实点了，但是这里面有问题，因为系统有误差，我们永远没办法知道真实点是多少，真实值是多少。

所以我们就只能退而求其次，对于我们的过程方程来说我们可以在k-1时的后验估计（上一次）进行线性化，也就是如下图所示

在这里面， $w_{k-1}$ 是误差，我们并不知道他这里面是多少，将其简单的假设为0

然后我们将其简化为 $\tilde{x}_{k}$ ,然后这里的A就是其对x求偏导再代入 $\tilde{x}_{k-1}$ ， ${u}_{k-1}$

举个例子

我们一个二维的系统

这样我们就得到了A矩阵在k时刻的样子，我们的A矩阵随着K的变化在不断变化当中，我们的每一步都要重新去计算A矩阵。

$w_{k}$ 就相当于f对w的一个偏导

$Z_{k}$ 我们就在 $\tilde{x}_{k}$ 进行线性化

然后我们的 $Z_{k}$ 就可以写成这种形式，我们和上面的处理一样，我们没有办法得到 $v_{k}$ 的结果，我们就假设他是0。

就这样我们就把整个的非线性系统在 $\hat{x}_{k-1}$ 和 $\tilde{x}_{k}$ 附近线性化了

在这里还有一个要注意的点就是，我们这里用大的矩阵W乘一个小w，他也是符合正态分布的，他的期望依然是0，但是他的协方差矩阵变成了这个，可以自己验证一些下。

我们用一个一维的来举例

我们可以看到这里的a乘x的期望可以把a带到外面来，然后就等于0了，下面的方差根据其定义就是a平方x的方差，左边的 $wqw^{T}$ 中的 $ww^{T}$ 就是起到一个平方的作用。

这里我们的 $Vv_{k}$ 的也是符合正态分布的这个形式

有了这样的结果我们就可以用推过的手段来推导了

首先我们这里的预测就跟之前不一样的，之前我们是线性化，这一次我们就用非线性的了来进行预测

修改之后就是这样

小橙不吃辣椒

关注

26
点赞
踩
20

收藏

觉得还不错? 一键收藏
0
评论
卡尔曼滤波器笔记——最详细

现在我们就是不知道是距离多少，就需要用到这个卡尔曼滤波器。这里的预测方程就是我们的状态方程这里的一般就是单位矩阵，或者是单位矩阵的一部分这里的一般就是状态变量的个数,就是你选择的观测值具体是什么形状要根据选择的状态变量以及要观测的目标值的的形状所以上面我们举的小车的例子的观测值就只有一个，那就是他的距离，所以就是1*1的矩阵，就是1，如果我们还想知道车速，那么就是2.就是2*1的矩阵。
复制链接

扫一扫