当前位置:首页 > 代码 > 正文

扩展卡尔曼滤波代码(卡尔曼滤波CSDN)

admin 发布:2023-02-09 17:45 27


今天给各位分享扩展卡尔曼滤波代码的知识,其中也会对卡尔曼滤波CSDN进行解释,如果能碰巧解决你现在面临的问题,别忘了关注本站,现在开始吧!

本文目录一览:

零基础读懂“扩展卡尔曼滤波”——中篇

本篇文章分上、中、下三篇,上篇从标准卡尔曼滤波开始,中篇加入更真实的系统模型,下篇从传感器的数据融合中实现扩展卡尔曼滤波。

现在重新回顾一下描述系统的状态方程和观测方程:

表示系统的当前状态, 表示系统的前一个状态; 是一个常量, 是系统当前状态的观测值; 是系统观测噪声;

这两个方程已经适用于大多数系统,但仍然不够普适性;现在依然以飞机的飞行为例,我们并没有考虑到飞行员对飞机的操作和控制,飞行员操作控制杆向前或向后移动,对飞机输入控制量,最终对飞机产生控制。我们将这个控制量称为 ,表示飞行员发送到飞机的控制信号的当前值,并且我们对这个控制量加上一个缩放因子 ,因此整个状态方程变为:

一般而言,除了噪声以外的任何信号都可以通过常量进行一定比例的缩放,因此观测方程也可以这样表示:

通过以上的方程我们更真实的描述一个系统的状态和观测,由此预测和更新方程也需要做相应的调整:

预测:

更新:

仍然以飞机飞行为例,我们增加一个控制信号,表示飞行员稳步拉回控制杆,并提高飞机飞行高度,原始信号为蓝色表示,观测值以红色表示,绿色信号为卡尔曼滤波后的值。

现在回想一下飞机下降时飞行高度的原始方程:

以更通用的方程表示为:

我们都知道飞机飞行的高度即海拔,可以看做是一种距离,距离总是和速度与时间相关的:

再进一步思考,当前距离和前一时刻的距离的关系:

下标 代表当前, 代表前一个。也就是说,飞机现在的位置等于前一时刻的位置加上刚刚飞行过的垂直距离。如果按照固定的时间间隔进行计算,比如说1秒,或者1分钟等,那么可以将上式简化为:

貌似与我们的状态方程更接近了一步,但是还差一些,接下来我们引入向量和矩阵。

通常情况下系统的状态并不是一个变量,比如飞机的高度和速度,那么我们就可以使用向量表示系统的状态:

符号 表示一个定义,系统状态定义为一个距离和速度的矢量。 当我们用一个矩阵去乘以一个向量时得到的结果依然是一个向量,比如:

因此,我们将系统状态方程中的常量 定义为一个矩阵,因为系统的状态并非是一种:

再次回顾一下系统状态方程:

将系统状态矢量代入:

从公式中可以清晰的看出距离等于前一刻的距离与速度和观测时间间隔的乘积之和,而速度和上一时刻的速度相等;这表明飞机是匀速上升或下降的,但现实中速度不可能做到匀速。当速度是变化的,即系统中还存在一个加速度变量,那么我们的系统状态方程可以表示为:

系统状态的修正公式:

为一个向量, 是一个矩阵;大家可能还记得,这个方程的原始形式是这样的:

其中 是一个控制信号, 是一个比例因子,系统的观测方程为:

其中 是观测量, 是由于传感器的不准确所带来的噪声。那么我们如何修正这些方程以适应新的向量矩阵形式呢?是的,线性代数让这些变得尤其简单。我们使用大写的字母写出缩放因子 和 ,使他们成为矩阵,而不是单一的标量值:

然后所有的变量包括状态、观测、噪声、控制等都被认为是向量,回顾一下我们前面的系统预测和更新方程:

预测:

更新:

大家可能会想,将这里所有的缩放因子都用大写表示,然后就可以了。然而事实并非如此,矩阵的乘法没有那么简单,这就是前面提到的为什么是 而不是 。 有时候不等于 ,有时候需要转置矩阵,通过在矩阵的旁边加上一个上标 来表示转置矩阵。转置矩阵是通过把每一行变成一列,每一列变成一行来实现的。看个实际例子:

由此,我们可以重写我们的预测方程: (关于卡尔曼滤波五个公式的由来,我会专门写一篇文章进行推导与分析)

我们已经知道 为何是一个矩阵,但是 为何也是一个矩阵呢?

我们的第二个更新方程可以表示为:

再看增益方程的原始形式:

它包含一个除法,我们瞬间想到了除法可以用乘法代替:

那么上面的方程可以写为:

我们现在把这些常量替换成矩阵:

如何计算 呢,求逆矩阵吗?是的,一个矩阵乘以它的逆矩阵后结果为一个单位矩阵

单位矩阵的定义如下,我们以一个 3x3 的矩阵举例:

现在我们的卡尔曼滤波方程完全以矩阵形式表示,

模型:

预测:

更新:

如果想在状态变量中添加以下额外的项似乎是一项很艰巨的任务。卡尔曼滤波最有价值的地方就是用在传感器的融合中。

回到飞机飞行的例子中,飞行员可以访问更多的信息,不仅仅是飞机的高度,仪表上可以显示飞机的水平速度、航向、经纬度、室外温湿度等信息。想象一下,假如一架飞机只有三个传感器,每一个传感器都有给定的状态,一个用于测量高度的气压计,一个用于航向的电子罗盘,一个测量空速的空速指示器等。

假设这些传感器是完全准确的,即没有任何噪声。那么我们的观测方程为:

将会变为:

假如我们向系统中加入了一个GPS,同样用于测量高度,那么我们的观测方程将会变为:

因此可以将多个传感器的读数结合起来,从而推断出某个状态的信息。就比如我们看病一样,对于重要的疾病总是寻求多个医生的诊断,对于一些重要的事情,最好有多个信息来源。

本篇先介绍这些,下一篇会以一个真实的传感器融合实例开始,并最终实现扩展卡尔曼滤波。

C51能不能实现卡尔曼滤波,如果可以能不能给我代码?

卡尔曼滤波只是一个算法,而C51是基于标准C语言扩展而来的,你只要明白卡尔曼滤波的数学表达算法,就能用C语言写出来卡尔曼滤波的程序,所以,C语言完全可以写出来卡尔曼滤波算法,C51自然也就能.

但是,这里有个但是!!!

C51虽然是基于标准C语言扩展的,但是,C51是用在51内核单片机上的,而以51内核为内核组成的单片机,大都硬件架构简单,内存容量小,没有专用的硬件乘法器,而且是8位的,基于以上原因,在实际应用中,51单片机是无法完成卡尔曼滤波的.

1 是没有专用硬件乘法/除法器

2 卡尔曼滤波是一种递归算法,需要极大的内存支持,51一般只有几K内存,不足以支持庞大的

卡尔曼滤波.算法

所以,如果你一定要卡尔曼滤波算法,换个强大的MCU吧

扩展卡尔曼滤波(EKF)算法详细推导及仿真(Matlab)

姓名:王柯祎

学号:20021110373T

转自 :

【嵌牛导读】介绍扩展卡尔曼滤波(EKF)算法的详细推导,局限性和MATLAB仿真。

【嵌牛鼻子】扩展卡尔曼滤波(EKF)

【嵌牛正文】

扩展卡尔曼滤波算法 是解决非线性状态估计问题最为直接的一种处理方法,尽管EKF不是最精确的”最优“滤波器,但在过去的几十年成功地应用到许多非线性系统中。所以在学习非线性滤波问题时应该先从EKF开始。

EKF算法是将非线性函数进行泰勒展开,然后省略高阶项,保留展开项的一阶项,以此来实现非线性函数线性化,最后通过卡尔曼滤波算法近似计算系统的状态估计值和方差估计值。

一、EKF算法详细推导

【注】EKF推导参考的是黄蔚的博士论文“CKF及鲁棒滤波在飞行器姿态估计中的应用研究”,论文中EKF,UKF和CKF等算法讲解的都很详细,值得一看。

我们把KF与EKF算法拿出来对比可以发现:

二、EKF算法局限性:

该算法线性化会引入阶段误差从而导致滤波精度下降,同时当初始状态误差较大或系统模型非线性程度较高时,滤波精度会受到严重影响甚至发散。

需要计算雅克比矩阵,复杂,计算量大,影响系统的实时性,还会导致EKF算法的数值稳定性差。

当系统存在模型失配,量测干扰,量测丢失,量测延迟或状态突变等复杂情况时,EKF算法鲁棒性差。

三、Matlab仿真:

clear all;clc;   close all;

tf = 50; 

Q = 10;w=sqrt(Q)*randn(1,tf); 

R = 1;v=sqrt(R)*randn(1,tf);

P =eye(1);

x=zeros(1,tf);

Xnew=zeros(1,tf);

x(1,1)=0.1; 

Xnew(1,1)=x(1,1);

z=zeros(1,tf);

z(1)=x(1,1)^2/20+v(1);

zjian=zeros(1,tf);

zjian(1,1)=z(1);

for k = 2 : tf

%%%%%%%%%%%%%%%模拟系统%%%%%%%%%%%%%%%

    x(:,k) = 0.5 * x(:,k-1) + (2.5 * x(:,k-1) / (1 + x(:,k-1).^2)) + 8 * cos(1.2*(k-1)) + w(k-1); 

    z(k) = x(:,k).^2 / 20 + v(k);

%%%%%%%%%%%%%%%EKF开始%%%%%%%%%%%%%%%

    Xpre = 0.5*Xnew(:,k-1)+ 2.5*Xnew(:,k-1)/(1+Xnew(:,k-1).^2) + 8 * cos(1.2*(k-1));  

    zjian =Xpre.^2/20;

    F = 0.5 + 2.5 * (1-Xnew.^2)/((1+Xnew.^2).^2);

    H = Xpre/10;    

    PP=F*P*F'+Q; 

    Kk=PP*H'*inv(H*PP*H'+R);

    Xnew(k)=Xpre+Kk*(z(k)-zjian);

    P=PP-Kk*H*PP;

end

  t = 2 : tf;  

 figure;   plot(t,x(1,t),'b',t,Xnew(1,t),'r*');  legend('真实值','EKF估计值');

仿真结果:

扩展卡尔曼滤波器的s函数编写?

H=[1,0,0];

K=P*H'*inv(H*P*H'+R);

D=u(1);

H=xp(1,1);

xp=xp+K*(D-H);

P=P-K*H*P;

H是一个3*1的矩阵,但在下面H=xp(1,1),这时又变成一个数了,后面xp=xp+K*(D-H);没有问题。但是P=P-K*H*P;这里就出现了矩阵维数不对。你换过来就行了。

扩展卡尔曼滤波代码的介绍就聊到这里吧,感谢你花时间阅读本站内容,更多关于卡尔曼滤波CSDN、扩展卡尔曼滤波代码的信息别忘了在本站进行查找喔。

版权说明:如非注明,本站文章均为 AH站长 原创,转载请注明出处和附带本文链接;

本文地址:http://ahzz.com.cn/post/36615.html


取消回复欢迎 发表评论:

分享到

温馨提示

下载成功了么?或者链接失效了?

联系我们反馈

立即下载