卡尔曼滤波算法与matlab实现

1、应用实例详细说明了卡尔曼滤波器及其算法实现标签：算法过滤器算法优化工作2012年5月14日，10:4875511人阅读评论(25)并收集报告分类：数据结构及其算法(4)为了更容易理解卡尔曼滤波器，本文将使用图像描述的方法来解释它，而不是像大多数参考书一样列出许多数学公式和数学符号。然而，他的五个公式是其核心内容。事实上，结合现代计算机，卡尔曼的程序相当简单，只要你理解他的五个公式。在介绍他的五个公式之前，让我们根据下面的例子一步一步地探索。假设我们想研究房间的温度。根据你的经验，这个房间的温度是恒定的，也就是说，下一分钟的温度等于当前分钟的温度(假设我们用一分钟作为时间单位)。如果你不100%

2、相信你的经验，可能会有几度的上下偏差。我们将这些偏差视为高斯白噪声，即这些偏差与前后时间无关，符合高斯分布。另外，我们在房间里放了一个温度计，但是这个温度计不准确，而且测量值会偏离实际值。我们也将这些偏差视为高斯白噪声。现在，对于某一分钟，我们有两个关于房间温度的值：基于经验的预测值(系统的预测值)和温度计的值(测量值)。接下来，我们将使用这两个值以及它们各自的噪声来估计房间的实际温度。假设我们想估计k时刻的实际温度。首先，你应该根据k-1的温度来预测k时刻的温度。因为你相信温度是恒定的，你可以得到k时间的温度预测值与k-1时间的温度预测值相同，假设它是23度，并且这个值的高斯噪声的偏差是5度

3、(5是这样获得的：如果k-1时间的估计最佳温度的偏差是3，你的预测的不确定性是4度，并且它们被平方，然后平方是5)。然后，你从温度计得到k时刻的温度值，假设它是25度，这个值的偏差是4度。有两个温度值用于估计k时刻的实际温度，分别为23度和25度。实际温度是多少？相信自己还是相信温度计？我们可以用它们的协方差来判断我们更相信谁。因为kg 2=5 2/(5 24 2)，所以kg=0.78，我们可以估计k时刻的实际温度为230.78 *(25-23)=24.56度。可以看出，估计的最佳温度值偏向温度计值，因为温度计的协方差相对较小。现在我们已经得到了时间k处的最佳温度值，下一步是输入时间k 1并进

4、行新的最佳估计。到目前为止，我们似乎还没有看到任何回归的迹象。顺便说一下，在输入k 1时间之前，我们必须计算k时间的最佳值(24.56度)的偏差。算法如下：(1千克)* 5 2) 0.5=2.35。这里，5是您在上述k时间预测的23度温度值的偏差，而获得的2.35是在输入k 1时间后在k时间估计的最佳温度值的偏差(对应于上述3)。这样，卡尔曼滤波器递归迭代协方差以估计最佳温度值。他跑得很快，只在最后一刻才保持协方差。上述千克是卡尔曼增益。他能在不同的时间改变自己的价值，这难道不令人惊讶吗？让我们言归正传，在真正的工程系统中讨论卡尔曼。3.卡尔曼滤波算法(卡尔曼滤波算法)在这一部分，我们将描述卡

5、尔曼滤波器从博士卡尔曼。下面的描述将涉及一些基本的概念知识，包括概率，随机变量，高斯或正态分布，状态空间模型等。然而，卡尔曼滤波器的详细证明不能在这里描述。首先，我们需要引入一个离散控制过程系统。系统可以用线性随机差分方程来描述：x(k)=a x(k-1) b u(k) w(k)加上系统的测量值：z(k)=高x(k)伏(k)在上述两个公式中，x(k)是k时刻的系统状态，u(k)是k时刻的系统控制量。a和b是系统参数，对于多模型系统，它们是矩阵。z(k)是k时刻的测量值，h是测量系统的参数，对于多测量系统，h是矩阵。w(k)和v(k)分别代表过程和测量噪声。假设它们是高斯白噪声，它们的协方差分别

6、是q和r(这里我们假设它们不随系统状态的变化而变化)。为了满足上述条件(线性随机微分系统，过程和测量是高斯白噪声)，卡尔曼滤波器是最好的信息处理器。让我们用它们来组合它们的协方差来估计系统的最佳输出(类似于上一节中的温度示例)。首先，我们应该使用系统的过程模型来预测系统的下一个状态。假设当前系统状态为k，根据系统模型，可以预测它将出现在基于系统先前状态的状态中：x(k|k-1)=a x(k-1|k-1) b u(k) .(1)在公式(1)中，x(k|k-1)是由先前状态预测的结果，x(k-1|k-1)是先前状态的最优结果，而u(k)是当前状态的控制量。如果没有控制量，可以是0。到目前为止，我们

7、的系统结果已经更新，但是对应于x(k|k-1)的协方差还没有更新。我们用p表示协方差：p(k | k-1)=a p(k-1 | k-1)a q(2)在公式(2)中，p(k|k-1)是对应于x(k|k-1)的协方差，p(k-1|k-1)是对应于x(k-1|k-1)的协方差，a表示a的转置矩阵，q是系统过程的协方差。等式1和等式2是卡尔曼滤波器的五个等式中的前两个，即系统的预测。现在我们有了当前状态的预测结果，然后我们收集当前状态的测量值。结合预测值和测量值，我们可以得到当前状态(k)的优化估计值x(k|k):x(k | k)=x(k | k-1)kg(k)(z(k)-h x(k | k-1)(3

8、)其中，千克是卡尔曼增益：kg(k)=p(k | k-1)h /(h p(k | k-1)h r)(4)到目前为止，我们已经得到了k状态下的最佳估计值x(k|k)。然而，为了使卡尔曼滤波器一直运行到系统过程结束，我们需要在k状态下更新x(k|k)的协方差：p(k | k)=(i-kg(k)h)p(k | k-1)(5)其中，i是1的矩阵，对于单一模型和单一测量，i=1。当系统进入k 1状态时，p(k|k)是公式(2)的p(k-1|k-1)。这样，算法可以执行自回归运算。卡尔曼滤波器的原理基本上描述了方程1、2、3、4和5是他的五个基本公式。根据这五个公式，计算机程序很容易实现。接下来，用mat

9、lab程序给出一个实际操作的例子。4.简单的例子(简单示例)这里我们结合第二和第三部分给出一个非常简单的例子来说明卡尔曼滤波器的工作过程。给出的例子是进一步描述第二部分的例子，并将附有程序模拟结果。根据第二节的描述，将房间视为一个系统，然后对系统进行建模。当然，我们看到的模型不需要非常精确。我们知道这个房间的温度和前一时刻的温度一样，所以a=1。没有控制量，所以u(k)=0。因此，得出的结论是：x(k|k-1)=x(k-1|k-1) .(6)等式(2)可以变为：p(k | k-1)=p(k-1 | k-1)q(7)因为测量值是温度计，并且直接对应于温度，所以h=1。等式3、4和5可以改变如下：

10、x(k | k)=x(k | k-1)kg(k)(z(k)-x(k | k-1)(8)公斤(k)=p(k | k-1)/(p(k | k-1)r)(9)p(k | k)=(1-kg(k)p(k | k-1)(10)现在我们模拟一组测量值作为输入。假设真实的室温为25度，我模拟了200个测量值，这些测量值的平均值为25度，但是添加了标准偏差为几度的高斯白噪声(图中的蓝线)。为了使卡尔曼滤波器工作，我们需要告诉卡尔曼两个零矩的初始值是x(0|0)和p(0|0)。不要太在意他们的价值观，只要给出一个，因为随着卡尔曼的工作，x将逐渐收敛。然而，对于p，一般不取0，因为这可能使卡尔曼完全相信你给定的x(

11、0|0)是最优系统，这使得算法无法收敛。我选择了x(0|0)=1度和p(0|0)=10。系统的实际温度是25度，在图中用黑线表示。图中的红线是卡尔曼滤波器输出的优化结果(该结果在算法中设置为q=1e-6和r=1e-1)。清楚的n=200。w(1)=0；w=随机(1，n)x(1)=0；a=1；对于k=2:nx(k)=a * x(k-1)w(k-1)；目标v=随机(1，n)；q1=标准(v)；rvv=q1。2；q2=标准(x)；rxx=q2。2；q3=标准(w)；rww=q3。2；c=0.2y=c * x v；p(1)=0；s(1)=0；t=2:n牛顿；p1(t)=a.2*p(t-1)rww；b(

12、t)=c*p1(t)/(c.2*p1(t)；s(t)=a * s(t-1)b(t)*(y(t)-a * c * s(t-1)；p(t)=p1(t)-c * b(t)* p1(t)；目标t=1:n牛顿；绘图(t，s，r，t，y，g，t，x，b)；matlab编制的卡尔曼滤波程序已经通过测试-下面还有一个matlab源程序，显示了更好的结果。清楚的clcn=300。con=25%室温，假设温度恒定%卡尔曼滤波器%x=零(1，n)；y=20.5 *兰登(1，n)con；%外加过程噪声状态输出x(1)=1；p=10q=cov(随机(1，n)；%过程噪声协方差r=cov(随机(1，n)；%观察到的噪声协

13、方差k=2 :牛x(k)=x(k-1)；%预先估计k时刻状态变量的值p=pq；%对应于估计值的协方差kg=p/(p . r)；%卡尔曼增益x(k)=x(k)kg *(y(k)-x(k)；p=(1千克)* p；目标%平滑度过滤器% % % % % % % % % % % % % % % % % % % % % % % % % %filter _ wid=10smooth_res=零(1，n)；i=滤波器_宽度1 :牛tempsum=0；用于j=i - filter_wid : i - 1temp sum=temp sum y(j)；目标smooth _ res(i)=tempsum/filter

14、 _ wid；目标%图(1)；% hist(y)；t=1:n牛顿；图(1)；expvalue=，n)；i=1:牛ex pvvalue(i)=con；目标绘图(t，expvalue，r，t，x，g，t，y，b，t，smooth_res，k)；图例(预期、估计、测量、平滑结果)；轴(0 n 20 30)xlabel(采样时间)；伊拉贝尔(室温)；标题(平滑滤波器vs卡尔曼滤波器)；卡尔曼滤波算法-核心公式推导介绍重建红旗写在前面：这是我的第一个专栏，我要感谢智虎提供了这样一个平台来与大家分享知识。我会不时更新文章，文章的内容应该集中在汽车动力学控制、汽车软件架构、控制器等方面。作为学校的老师，许多理解可能是不完整和不正确的。每个人都有不同的意见要讨论。谢谢你！-卡尔曼滤波算法很棒，因为有一大堆公式和符号，看起来棒极了，但乍看之下，那些不懂的人棒极了！本文推导了卡尔曼滤波算法的核心公式，以免每个人被其华丽的外表所吓倒。(之后，我打算写关于ekf和ukf的非线性案例，并全面介绍卡尔曼滤波算法的应用。感兴趣的人可以关注这个专栏。)-好吧，言归正传。本文假设读者对卡尔曼滤波算法有一个初步的了解，知道它能做什么