卡尔曼滤波器算法浅析及matlab实战

最新推荐文章于 2024-07-31 16:49:23 发布
最新推荐文章于 2024-07-31 16:49:23 发布
阅读量2.2k 收藏 13
点赞数 2
分类专栏： 机器学习 文章标签： 机器学习

卡尔曼滤波器算法浅析及matlab实战
http://blog.csdn.net/llp1992/article/details/45370025
卡尔曼滤波器是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。而且由于观测包含系统的噪声和干扰的影响，所以最优估计也可看做是滤波过程。

卡尔曼滤波器的核心内容就是5条公式，计算简单快速，适合用于少量数据的预测和估计。

下面我们用一个例子来说明一下卡尔曼算法的应用。

假设我们想在有一辆小车，在 t 时刻其速度为 Vt ，位置坐标为 Pt，ut 表示 t 时刻的加速度，那么我们可以用Xt表示 t 时刻的状态，如下：


则我们可以得到，由t-1 时刻到 t 时刻，位置以及速度的转换如下：


用向量表示上述转换过程，如下：


如下图：


那么我们可以得到如下的状态转移公式：


 (1)

其中矩阵 F 为状态转移矩阵，表示如何从上一状态来推测当前时刻的状态，B 为控制矩阵，表示控制量u如何作用于当前矩阵，上面的公式 x 有顶帽子，表示只是估计值，并不是最优的。

有了状态转移公式就可以用来推测当前的状态，但是所有的推测都是包含噪声的，噪声越大，不确定越大，协方差矩阵用来表示这次推测带来的不确定性

协方差矩阵

假设我们有一个一维的数据，这个数据每次测量都不同，我们假设服从高斯分布，那么我们可以用均值和方差来表示该数据集，我们将该一维数据集投影到坐标轴上，如下图：


可以看到，服从高斯分布的一维数据大部分分布在均值附近。

现在我们来看看服从高斯分布的二维数据投影到坐标轴的情况，如下图：


二维数据比一维数据稍微复杂一点，投影后有3种情况，分别是： 
左图：两个维的数据互不相关； 
中图：两个维的数据正相关，也就是 y 随着 x 的增大而增大（假设两个维分别为 x 和 y） 
右图：两个维的数据负相关，也就是 y 随着 x 的增大而减小。

那怎么来表示两个维的数据的相关性呢？答案就是协方差矩阵。


状态协方差矩阵传递

在公式（1）之中，我们已经得到了状态的转移公式，但是由上面可知，二维数据的协方差矩阵对于描述数据的特征是很重要的，那么我们应该如何更新或者说传递我们的二维数据的协方差矩阵呢？假如我们用 P 来表示状态协方差，即


那么加入状态转换矩阵 F ，得到


 (2)

也即：


因此我们便得到了协方差的转换公式。

现在我们得到了两个公式，运用这两个公式能够对现在状态进行预测。按照我们的正常思路来理解，预测结果不一定会对嘛，肯定有误差。而且在我们大多数回归算法或者是拟合算法中，一般思路都是先预测，然后看看这个预测结果跟实际结果的误差有多大，再根据这个误差来调整预测函数的参数，不断迭代调整参数直到预测误差小于一定的阈值。

卡尔曼算法的迭代思想也类似，不过这里根据误差调整的是状态 X 。

在这里，我们的实际数据就是 Z， 如下图：


其中，矩阵 H 为测量系统的参数，即观察矩阵，v 为观测噪声， 其协方差矩阵为R

那么我们的状态更新公式如下：


其中K 为卡尔曼系数， Z-Hx 则为残差，也就是我们说的，预测值与实际值的误差。

K的作用：

1.K 权衡预测协方差P和观察协方差矩阵R那个更加重要，相信预测，残差的权重小，相信观察，残差权重大，由 K 的表达是可以退出这个结论 
2,将残差的表现形式从观察域转换到状态域（残差与一个标量，通过K 转换为向量），由 状态 X 的更新公式可得到该结论。

至此，我们已经得到了 t 状态下的最优估计值 Xt。但为了能让我们的迭代算法持续下去，我们还必须更新状态协方差的值。

状态协方差的更新


以上就是卡尔曼滤波算法的思想，只有简单的 5 条公式，总结如下：


Matlab 实现

<code class="language-matlab hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;"><span class="hljs-function" style="box-sizing: border-box;"><span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">function</span> <span class="hljs-title" style="box-sizing: border-box;">kalmanFiltering</span></span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%%</span>
clc
close all

<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%%</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%   Description : kalmanFiltering</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%   Author : Liulongpo</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%   Time：2015-4-29 16:42:34</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%</span>

<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%%</span>
Z=(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>:<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>:<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">200</span>); <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%观测值  汽车的位置  也就是我们要修改的量</span>
noise=<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">randn</span>(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>,<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">100</span>); <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%方差为1的高斯噪声</span>
Z=Z+noise;
X=<span class="hljs-matrix" style="box-sizing: border-box;">[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span> ; <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span> ]</span>; <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%初始状态</span>
P=<span class="hljs-matrix" style="box-sizing: border-box;">[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>]</span>; <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%状态协方差矩阵</span>
F=<span class="hljs-matrix" style="box-sizing: border-box;">[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>]</span>; <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%状态转移矩阵</span>
Q=<span class="hljs-matrix" style="box-sizing: border-box;">[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0001</span>,<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span> , <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0001</span>]</span>; <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%状态转移协方差矩阵</span>
H=<span class="hljs-matrix" style="box-sizing: border-box;">[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>,<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>]</span>; <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%观测矩阵</span>
R=<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>; <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%观测噪声方差</span>
figure;
hold on;
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">for</span> <span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">i</span> = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>:<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">100</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%基于上一状态预测当前状态  </span>
X_ = F*X;
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">% 更新协方差  Q系统过程的协方差  这两个公式是对系统的预测</span>
P_ = F*P*<span class="hljs-transposed_variable" style="box-sizing: border-box;">F'</span>+Q;
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">% 计算卡尔曼增益</span>
K = P_*<span class="hljs-transposed_variable" style="box-sizing: border-box;">H'</span>/(H*P_*<span class="hljs-transposed_variable" style="box-sizing: border-box;">H'</span>+R);
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">% 得到当前状态的最优化估算值  增益乘以残差</span>
X = X_+K*(Z(<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">i</span>)-H*X_);
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%更新K状态的协方差</span>
P = (<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">eye</span>(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>)-K*H)*P_;
scatter(X(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>), X(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>),<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">4</span>); <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%画点，横轴表示位置，纵轴表示速度</span>
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">end</span>

<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">end</span>
</code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li></ul>

效果如下


其中 x 轴为位置，y轴为速度。 
在代码中，我们设定x的变化是 1:2:200，则速度就是2，可以由上图看到，值经过几次迭代，速度就基本上在 2 附近摆动，摆动的原因是我们加入了噪声。

接下来来看一个实际例子。

我们的数据为 data = [149.360 , 150.06, 151.44, 152.81,154.19 ,157.72]; 
这是运用光流法从视频中获取角点的实际x轴坐标，总共有6个数据，也就是代表了一个点的连续6帧的x轴坐标。接下来这个例子，我们将实现用5帧的数据进行训练，然后预测出第6帧的x轴坐标。

在上一个matlab例子中，我们的训练数据比较多，因此我们的初始状态设置为[0,0]，也就是位置为0，速度为0，在训练数据比较多的情况下，初始化数据为0并没有关系，因为我们在上面的效果图中可以看到，算法的经过短暂的迭代就能够发挥作用。

但在这里，我们的训练数据只有5帧，所以为了加快训练，我们将位置状态初始化为第一帧的位置，速度初始化为第二帧与第一帧之差。

代码如下：

KF.m

<code class="language-matlab hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;"><span class="hljs-function" style="box-sizing: border-box;"><span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">function</span> <span class="hljs-params" style="color: rgb(102, 0, 102); box-sizing: border-box;">[predData,dataX]</span> = <span class="hljs-title" style="box-sizing: border-box;">KF</span><span class="hljs-params" style="color: rgb(102, 0, 102); box-sizing: border-box;">(dataZ)</span></span>

<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%%</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%   Description : kalmanFiltering</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%   Author : Liulongpo</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%   Time：2015-4-29 16:42:34</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%%</span>
Z = <span class="hljs-transposed_variable" style="box-sizing: border-box;">dataZ'</span>;
len = <span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">length</span>(Z);
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%Z=(1:2:200); %观测值  汽车的位置  也就是我们要修改的量</span>
noise=<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">randn</span>(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>,len); <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%方差为1的高斯噪声</span>
dataX = <span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">zeros</span>(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>,len);
Z=Z+noise;
X=<span class="hljs-matrix" style="box-sizing: border-box;">[Z(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>) ; Z(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>)-Z(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>) ]</span>; <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%初始状态  分别为 位置 和速度</span>
P=<span class="hljs-matrix" style="box-sizing: border-box;">[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>]</span>; <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%状态协方差矩阵</span>
F=<span class="hljs-matrix" style="box-sizing: border-box;">[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>;<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span> <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>]</span>; <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%状态转移矩阵</span>
Q=<span class="hljs-matrix" style="box-sizing: border-box;">[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0001</span>,<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>;<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span> , <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0.0001</span>]</span>; <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%状态转移协方差矩阵</span>
H=<span class="hljs-matrix" style="box-sizing: border-box;">[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>,<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">0</span>]</span>; <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%观测矩阵</span>
R=<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>; <span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%观测噪声方差</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%figure;</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%hold on;</span>
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">for</span> <span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">i</span> = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>:len
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%基于上一状态预测当前状态  </span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">% 2x1  2x1</span>
X_ = F*X;
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">% 更新协方差  Q系统过程的协方差  这两个公式是对系统的预测</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%   2x1  2x1  1x2  2x2</span>
P_ = F*P*<span class="hljs-transposed_variable" style="box-sizing: border-box;">F'</span>+Q;
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">% 计算卡尔曼增益</span>
K = P_*<span class="hljs-transposed_variable" style="box-sizing: border-box;">H'</span>/(H*P_*<span class="hljs-transposed_variable" style="box-sizing: border-box;">H'</span>+R);
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">% 得到当前状态的最优化估算值  增益乘以残差</span>
X = X_+K*(Z(<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">i</span>)-H*X_);
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%更新K状态的协方差</span>
P = (<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">eye</span>(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>)-K*H)*P_;
dataX(:,<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">i</span>) = <span class="hljs-matrix" style="box-sizing: border-box;">[X(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>);X(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">2</span>)]</span>;
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%scatter(X(1), X(2),4); %画点，横轴表示位置，纵轴表示速度</span>
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">end</span>
predData = F*X;
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">end</span></code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li><li style="box-sizing: border-box; padding: 0px 5px;">29</li><li style="box-sizing: border-box; padding: 0px 5px;">30</li><li style="box-sizing: border-box; padding: 0px 5px;">31</li><li style="box-sizing: border-box; padding: 0px 5px;">32</li><li style="box-sizing: border-box; padding: 0px 5px;">33</li><li style="box-sizing: border-box; padding: 0px 5px;">34</li><li style="box-sizing: border-box; padding: 0px 5px;">35</li><li style="box-sizing: border-box; padding: 0px 5px;">36</li><li style="box-sizing: border-box; padding: 0px 5px;">37</li><li style="box-sizing: border-box; padding: 0px 5px;">38</li><li style="box-sizing: border-box; padding: 0px 5px;">39</li><li style="box-sizing: border-box; padding: 0px 5px;">40</li><li style="box-sizing: border-box; padding: 0px 5px;">41</li></ul>

testKF.m

<code class="language-matlab hljs  has-numbering" style="display: block; padding: 0px; color: inherit; box-sizing: border-box; font-family: 'Source Code Pro', monospace;font-size:undefined; white-space: pre; border-radius: 0px; word-wrap: normal; background: transparent;"><span class="hljs-function" style="box-sizing: border-box;"><span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">function</span> <span class="hljs-title" style="box-sizing: border-box;">testKF</span></span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%% </span>
clc
close all
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%%</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%data = load('D:\\a.txt');</span>
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%data = [149.360 , 150.06, 151.44, 152.81,154.19,157.72,157.47,159.33,153.66];</span>
data = <span class="hljs-matrix" style="box-sizing: border-box;">[<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">149.360</span> , <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">150.06</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">151.44</span>, <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">152.81</span>,<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">154.19</span> ,<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">157.72</span>]</span>;
<span class="hljs-matrix" style="box-sizing: border-box;">[predData , DataX]</span> = KF(<span class="hljs-transposed_variable" style="box-sizing: border-box;">data'</span>);
error = DataX(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>,:) - data;

<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">i</span> = <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>:<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">length</span>(data);
figure
subplot <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">311</span>
scatter(<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">i</span>,data,<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>),title(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">'原始数据'</span>)
subplot <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">312</span>
scatter(<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">i</span>,DataX(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>,:),<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>),title(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">'预测数据'</span>)
subplot <span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">313</span>
scatter(<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">i</span>,error,<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>),title(<span class="hljs-string" style="color: rgb(0, 136, 0); box-sizing: border-box;">'预测误差'</span>)
predData(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>)
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%{</span>
scatter(<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">i</span>,error,<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>);
figure
scatter(<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">i</span>,data,<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>)
figure
scatter(<span class="hljs-built_in" style="color: rgb(102, 0, 102); box-sizing: border-box;">i</span>,predData(<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">1</span>,:),<span class="hljs-number" style="color: rgb(0, 102, 102); box-sizing: border-box;">3</span>)
<span class="hljs-comment" style="color: rgb(136, 0, 0); box-sizing: border-box;">%}</span>
<span class="hljs-keyword" style="color: rgb(0, 0, 136); box-sizing: border-box;">end</span></code><ul class="pre-numbering" style="box-sizing: border-box; position: absolute; width: 50px; top: 0px; left: 0px; margin: 0px; padding: 6px 0px 40px; border-right-width: 1px; border-right-style: solid; border-right-color: rgb(221, 221, 221); list-style: none; text-align: right; background-color: rgb(238, 238, 238);"><li style="box-sizing: border-box; padding: 0px 5px;">1</li><li style="box-sizing: border-box; padding: 0px 5px;">2</li><li style="box-sizing: border-box; padding: 0px 5px;">3</li><li style="box-sizing: border-box; padding: 0px 5px;">4</li><li style="box-sizing: border-box; padding: 0px 5px;">5</li><li style="box-sizing: border-box; padding: 0px 5px;">6</li><li style="box-sizing: border-box; padding: 0px 5px;">7</li><li style="box-sizing: border-box; padding: 0px 5px;">8</li><li style="box-sizing: border-box; padding: 0px 5px;">9</li><li style="box-sizing: border-box; padding: 0px 5px;">10</li><li style="box-sizing: border-box; padding: 0px 5px;">11</li><li style="box-sizing: border-box; padding: 0px 5px;">12</li><li style="box-sizing: border-box; padding: 0px 5px;">13</li><li style="box-sizing: border-box; padding: 0px 5px;">14</li><li style="box-sizing: border-box; padding: 0px 5px;">15</li><li style="box-sizing: border-box; padding: 0px 5px;">16</li><li style="box-sizing: border-box; padding: 0px 5px;">17</li><li style="box-sizing: border-box; padding: 0px 5px;">18</li><li style="box-sizing: border-box; padding: 0px 5px;">19</li><li style="box-sizing: border-box; padding: 0px 5px;">20</li><li style="box-sizing: border-box; padding: 0px 5px;">21</li><li style="box-sizing: border-box; padding: 0px 5px;">22</li><li style="box-sizing: border-box; padding: 0px 5px;">23</li><li style="box-sizing: border-box; padding: 0px 5px;">24</li><li style="box-sizing: border-box; padding: 0px 5px;">25</li><li style="box-sizing: border-box; padding: 0px 5px;">26</li><li style="box-sizing: border-box; padding: 0px 5px;">27</li><li style="box-sizing: border-box; padding: 0px 5px;">28</li></ul>

效果如下：


预测结果为： 155.7493 ，跟实际结果 157.72 仅有1.9 的误差，可以看到，卡尔曼滤波器算法对于少量数据的预测效果还是挺不错的。当然，预测位置的同时，我们也得到了预测速度。

参考文献： 视频: 卡尔曼滤波的原理以及在MATLAB中的实现

zdy0_2004
关注
专栏目录
卡尔曼滤波器求速度matlab,卡尔曼滤波器算法浅析及matlab实战
weixin_42509507的博客
03-19 997
原标题：卡尔曼滤波器算法浅析及matlab实战作者：Liu_LongPo出处：Liu_LongPo的博客卡尔曼滤波器是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。而且由于观测包含系统的噪声和干扰的影响，所以最优估计也可看做是滤波过程。卡尔曼滤波器的核心内容就是5条公式，计算简单快速，适合用于少量数据的预测和估计。下面我们用一个例子来说明一下卡尔曼算法的应用。假...
机器学习算法原理与实践（三）、卡尔曼滤波器算法浅析及matlab实战
Liu_LongPo的专栏
04-29 2万+
卡尔曼滤波器是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。而且由于观测包含系统的噪声和干扰的影响，所以最优估计也可看做是滤波过程。卡尔曼滤波器的核心内容就是5条公式，计算简单快速，适合用于少量数据的预测和估计。下面我们用一个例子来说明一下卡尔曼算法的应用。假设我们想在有一辆小车，在 t 时刻其速度为 Vt ，位置坐标为 Pt，ut 表示 t 时刻的加速度，那么我
【算法】卡尔曼滤波
最新发布
wnm23的专栏
07-31 1322
卡尔曼滤波是一种强大的递归滤波器，适用于处理线性或非线性动态系统中的不确定性。卡尔曼滤波的原理、实现和应用场景，多语言代码示例。在实际应用中，卡尔曼滤波可以帮助我们准确地估计系统状态，提高决策和控制的精度。随着技术的发展，卡尔曼滤波及其变种在各个领域的应用将更加广泛。
控制算法学习 一、卡尔曼滤波（1）以小车为例
qq_41035283的博客
10-20 3212
控制算法学习 一、卡尔曼滤波前言线性系统状态方程和观测方程卡尔曼滤波过程 前言 卡尔曼滤波（Kalman Filter, KF）是最经典的传感器滤波/去噪/估计/预测算法，适用于线性系统的最优状态估计，并且衍生出扩展卡尔曼滤波（EKF）、迭代卡尔曼滤波（IKF），无迹卡尔曼滤波（UKF）和容积卡尔曼滤波（CKF）等。 本文将非常简单的介绍和推导卡尔曼滤波，加深KF的理解记忆。 线性系统 什么是线性系统？控制领域中，系统输入输出满足叠加原理（叠加性和齐次性）就是线性系统。 一个简单的例子，一维路径上运行的匀速
智慧交通day02-车流量检测实现05：卡尔曼滤波器实践(小车模型)
小李的研究生学习日记
11-11 2419
1.filterpy FilterPy是一个实现了各种滤波器的Python模块，它实现著名的卡尔曼滤波和粒子滤波器。我们可以直接调用该库完成卡尔曼滤波器实现。其中的主要模块包括： filterpy.kalman 该模块主要实现了各种卡尔曼滤波器，包括常见的线性卡尔曼滤波器，扩展卡尔曼滤波器等。 filterpy.common 该模块主要提供支持实现滤波的各种辅助函数，其中计算噪声矩阵的函数，线性方程离散化的函数等。 filterpy.stats 该模块提供与滤波相关的
新手平衡小车的卡尔曼滤波算法总结
11-12
新手平衡小车的卡尔曼滤波算法总结，新手平衡小车的卡尔曼滤波算法总结
卡尔曼滤波——二轮平衡小车
weixin_45002397的博客
03-16 802
二轮平衡小车——滤波算法
卡尔曼滤波器测试：卡尔曼滤波器基本算法-matlab开发
05-29
描述中提到的"kalman_filter_tester"可能是一个MATLAB程序，用于测试和验证卡尔曼滤波器的基本算法。它可能包含了一个简单的系统模型，通过模拟数据或实际传感器输入来运行滤波器，并对结果进行分析。GitHub链接...
标量卡尔曼滤波器：实现标量卡尔曼滤波器的代码-matlab开发
05-30
标量卡尔曼滤波器（Scalar Kalman Filter）是一种基于概率推理的算法，常用于处理线性高斯系统的状态估计问题。在MATLAB环境中实现标量卡尔曼滤波器，可以为许多实际应用提供强大的工具，例如传感器融合、动态系统...
卡尔曼滤波器-误差状态卡尔曼滤波器算法实现.zip
03-14
本文件主要探讨的是如何实现误差状态卡尔曼滤波器算法。 首先，理解卡尔曼滤波器的基本原理至关重要。卡尔曼滤波器是一种递归的估计算法，它结合了系统的动态模型和观测数据，以提供最佳线性无偏估计（BLUE）——即...
一维小车卡尔曼滤波算法_car_graphgfi_matlab_kalman_
09-30
通过一维小车运动来初步介绍卡尔曼算法的简单应用
卡尔曼滤波matlab
02-27
卡尔曼算法matlab代码，包含扩展卡尔曼和无迹卡尔曼，测试通过。
卡尔曼滤波Matlab
12-29
在Matlab里面，描述的卡尔曼滤波。清楚的描述了卡尔曼滤波的表达试
卡尔曼滤波-Matlab程序
12-26
卡尔曼滤波程序Matlab实现。 Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态. 由于, 它便于计算机编程实现, 并能够对现场采集的数据进行实时的更新和处理, Kalman滤波是目前应用最为广泛的滤波方法, 在通信, 导航, 制导与控制等多领域得到了较好的应用。
卡尔曼滤波 Matlab程序
11-07
代码为书籍《卡尔曼滤波原理及应用》配套的代码，可用于卡尔曼滤波的学习
徐亦达机器学习：Kalman Filter 卡尔曼滤波笔记 (一)
To be a better man
09-12 419
Pxt​|xt−1​)Pyt​|xt​)Px1​)AXt−1​Xt​​πNAXt−1​BQ)NHXt​CR)Nμ0​ϵ0​)fxt−1​)gyt​)fx1​)⎩⎨⎧​Py1​...yt​−−evaluationargmentθlogPy1...yt​∣θ−−parameterl。
Kalman滤波器从原理到实现
热门推荐
xiahouzuoxin
09-26 14万+
目录包括：Kalman滤波器的历史渊源、从牛顿到卡尔曼、一场递推的游戏、均方误差中的门道、Matlab程序看过来、Kalman滤波C程序、参考资料
小车直线运动的C++卡尔曼滤波，以及如何升级为扩展卡尔曼滤波
qq_42683787的博客
12-14 1713
场景描述：基于视觉的无人机移动平台自主降落，其中对小车的位置使用卡尔曼滤波状态估计。
卡尔曼滤波 matlab 代码
10-30
卡尔曼滤波是一种用于估计系统状态的算法，它可以通过对系统的测量值和模型进行融合，得到更加准确的状态估计。以下是一个简单的卡尔曼滤波 Matlab 代码样例： ``` %初始化 x = [0; 0]; %状态向量 P = [1 0; 0 1]; %状态协方差矩阵 F = [1 1; 0 1]; %状态转移矩阵 Q = [0.1 0; 0 0.1]; %过程噪声协方差矩阵 H = [1 0]; %观测矩阵 R = 1; %观测噪声协方差 %生成观测数据 z = sin(1:0.1:10) + randn(1, 91); %卡尔曼滤波 for k = 1:length(z) %预测 x = F * x; P = F * P * F' + Q; %更新 K = P * H' / (H * P * H' + R); x = x + K * (z(k) - H * x); P = (eye(2) - K * H) * P; %保存结果 x_est(:, k) = x; end %绘图 plot(1:length(z), z, 'b', 1:length(z), x_est(1, :), 'r'); legend('观测值', '估计值'); ``` 该代码实现了一个简单的一维卡尔曼滤波器，用于估计一个正弦信号的状态。其中，x 表示状态向量，P 表示状态协方差矩阵，F 表示状态转移矩阵，Q 表示过程噪声协方差矩阵，H 表示观测矩阵，R 表示观测噪声协方差。在每个时间步骤中，先进行预测，然后根据观测值进行更新，最后保存估计结果并绘制图像。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值