扩展卡尔曼滤波代码（详解卡尔曼滤波）

本篇文章给大家谈谈扩展卡尔曼滤波代码，以及详解卡尔曼滤波对应的知识点，希望对各位有所帮助，不要忘了收藏本站喔。

本文目录一览：

• 1、扩展卡尔曼滤波器的s函数编写？
• 2、扩展卡尔曼滤波（EKF）算法详细推导及仿真（Matlab）
• 3、C51能不能实现卡尔曼滤波，如果可以能不能给我代码？

扩展卡尔曼滤波器的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;这里就出现了矩阵维数不对。你换过来就行了。

扩展卡尔曼滤波（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估计值');

仿真结果：

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

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

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

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

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

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

卡尔曼滤波.算法

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

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