精华内容
下载资源
问答
  • 扩展卡尔曼滤波MATLAB代码 % UKF Unscented Kalman Filter for nonlinear dynamic systems % [x, P] = ukf(f,x,P,h,z,Q,R) returns state estimate, x and state covariance, P
  • 卡尔曼滤波matlab代码,很不错的一段代码,扩展卡尔曼在参数估计等领域应用广泛,希望可以帮到你!
  • 姿态确定的扩展卡尔曼滤波Matlab

    热门讨论 2011-03-21 10:15:48
    在姿态确定的四元数方程基础上,采用扩展卡尔曼滤波
  • 惯性导航扩展卡尔曼滤波MATLAB
  • matlab卡尔曼滤波和扩展卡尔曼滤波(KF&EKF)例程,可供学习参考使用,谢谢! 希望能帮到你。。。。。。。。。。。
  • TDOA_AOA定位的扩展卡尔曼滤波算法MATLAB源代码
  • 本文件为扩展卡尔曼滤波matlab代码,给需要学习扩展卡尔曼滤波的朋友使用,m文件直接运行
  • 扩展卡尔曼滤波算法的matlab程序
  • 基于扩展卡尔曼滤波算法的飞机姿态控制matlab程序
  • 卡尔曼滤波matlab

    2018-02-27 19:29:23
    卡尔曼算法matlab代码,包含扩展卡尔曼和无迹卡尔曼,测试通过。
  • 试利用扩展卡尔曼滤波理论求出的最优估计。 要求: (1)利用Matlab或Python 编写仿真程序。 (2)在同一张图中,给出的真值和估计值曲线。 (3)给出的真值与估计值之间的误差曲线变化图,并求出误差的均值和方差...
  • 前段时间给帮助同学做了个毕业设计,基于matlab扩展卡尔曼滤波,上传上来供大家学习参考,直接打开就可以正常运行。
  • TDOA/AOA定位的扩展卡尔曼滤波算法MATLAB源代码TDOA/AOA是无线定位领域里使用得比较多的一种定位体制,扩展卡尔曼滤波器是最经典的非线性滤波算法,可用于目标的定位和动态轨迹跟踪。function[MX,MY,SS]=...

    TDOA/AOA定位的扩展卡尔曼滤波算法MATLAB源代码

    TDOA/AOA是无线定位领域里使用得比较多的一种定位体制,扩展卡尔曼滤波器是最经典的非线性滤波算法,可用于目标的定位和动态轨迹跟踪。

    function

    [MX,MY,SS]=ExtendedKalmanFilter(D1,D2,D3,A1,A2,A3,Flag1,FLAG2,S0,P0,SigmaR,SigmaA OA)

    %% TDOA/AOA定位的扩展卡尔曼滤波定位算法

    % GreenSim团队——专业级算法设计&代写程序

    % 欢迎访问GreenSim团队主页→http://www.doczj.com/doc/f2093bc108a1284ac85043e0.html/greensim

    %% 输入参数列表

    % D1 基站1和移动台之间的距离

    % D2 基站2和移动台之间的距离

    % D3 基站3和移动台之间的距离

    % A1 基站1测得的角度值

    % A2 基站2测得的角度值

    % A3 基站3测得的角度值

    % Falg1 1×1矩阵,取值1,2,3,表明是以哪一个基站作为基准站计算TDOA数据的% FLAG2 N×3矩阵,取值0和1,每一行表示该时刻各基站的AOA是否可选择,% 1表示选择AOA数据,FLAG2并非人为给定,而是由LOS/NLOS检测模块确定

    % S0 初始状态向量,4×1矩阵

    % P0 预测误差矩阵的初始值,4×4的矩阵

    % SigmaR 无偏/有偏卡尔曼输出距离值的方差,由事先统计得到

    % SigmaAOA 选择AOA数据的方差,生成AOA数据的规律已知,因此可以确定

    %% 输出参数列表

    % MX

    % MY

    %% 第一步:计算TDOA数据

    if Flag1==1

    TDOA1=D2-D1;

    TDOA2=D3-D1;

    elseif Flag1==2

    TDOA1=D1-D2;

    TDOA2=D3-D2;

    elseif Flag1==3

    TDOA1=D1-D3;

    TDOA2=D2-D3;

    else

    error('Flag1输入有误,它只能取1,2,3');

    end

    %% 第二步:构造两个固定的矩阵

    展开全文
  • 本资源是关于扩展卡尔曼滤波算法的matlab程序
  • 扩展卡尔曼滤波算法的matlab程序》由会员分享,可在线阅读,更多相关《扩展卡尔曼滤波算法的matlab程序(7页珍藏版)》请在人人文库网上搜索。1、clear allv=150; %目标速度v_sensor=0;%传感器速度t=1; %扫描周期...

    《扩展卡尔曼滤波算法的matlab程序》由会员分享,可在线阅读,更多相关《扩展卡尔曼滤波算法的matlab程序(7页珍藏版)》请在人人文库网上搜索。

    1、clear allv=150; %目标速度v_sensor=0;%传感器速度t=1; %扫描周期xradarpositon=0; %传感器坐标yradarpositon=0; %ppred=zeros(4,4);Pzz=zeros(2,2);Pxx=zeros(4,2);xpred=zeros(4,1);ypred=zeros(2,1);sumx=0;sumy=0;sumxukf=0;sumyukf=0;sumxekf=0;sumyekf=0; %统计的初值L=4;alpha=1;kalpha=0;belta=2;ramda=3-L;azimutherror=0.015; %方位均方误差ran。

    2、geerror=100; %距离均方误差processnoise=1; %过程噪声均方差tao=t3/3 t2/2 0 0;t2/2 t 0 0;0 0 t3/3 t2/2;0 0 t2/2 t; % the input matrix of process G=t2/2 0 t 0 0 t2/20 t ;a=35*pi/180;a_v=5/100;a_sensor=45*pi/180;x(1)=8000; %初始位置y(1)=12000;for i=1:200x(i+1)=x(i)+v*cos(a)*t;y(i+1)=y(i)+v*sin(a)*t; endfor i=1:200 xradar。

    3、positon=0;yradarpositon=0;Zmeasure(1,i)=atan(y(i)-yradarpositon)/(x(i)-xradarpositon)+random(Normal,0,azimutherror,1,1); Zmeasure(2,i)=sqrt(y(i)-yradarpositon)2+(x(i)-xradarpositon)2)+random(Normal,0,rangeerror,1,1); xx(i)=Zmeasure(2,i)*cos(Zmeasure(1,i);%观测值yy(i)=Zmeasure(2,i)*sin(Zmeasure(1,i); me。

    4、asureerror=azimutherror2 0;0 rangeerror2;processerror=tao*processnoise;vNoise = size(processerror,1);wNoise = size(measureerror,1);A=1 t 0 0;0 1 0 0;0 0 1 t;0 0 0 1;Anoise=size(A,1);for j=1:2*L+1Wm(j)=1/(2*(L+ramda);Wc(j)=1/(2*(L+ramda);endWm(1)=ramda/(L+ramda);Wc(1)=ramda/(L+ramda);%+1-alpha2+belta。

    5、; %权值if i=1 xerror=rangeerror2*cos(Zmeasure(1,i)2+Zmeasure(2,i)2*azimutherror2*sin(Zmeasure(1,i)2;yerror=rangeerror2*sin(Zmeasure(1,i)2+Zmeasure(2,i)2*azimutherror2*cos(Zmeasure(1,i)2;xyerror=(rangeerror2-Zmeasure(2,i)2*azimutherror2)*sin(Zmeasure(1,i)*cos(Zmeasure(1,i);P=xerror xerror/t xyerror xye。

    6、rror/t;xerror/t 2*xerror/(t2) xyerror/t 2*xyerror/(t2);xyerror xyerror/t yerror yerror/t;xyerror/t 2*xyerror/(t2) yerror/t 2*yerror/(t2);xestimate=Zmeasure(2,i)*cos(Zmeasure(1,i) 0 Zmeasure(2,i)*sin(Zmeasure(1,i) 0 ;end cho=(chol(P*(L+ramda);%for j=1:L xgamaP1(:,j)=xestimate+cho(:,j);xgamaP2(:,j)=xe。

    7、stimate-cho(:,j);endXsigma=xestimate xgamaP1 xgamaP2; F=A; Xsigmapre=F*Xsigma;xpred=zeros(Anoise,1); for j=1:2*L+1xpred=xpred+Wm(j)*Xsigmapre(:,j);endNoise1=Anoise; ppred=zeros(Noise1,Noise1); for j=1:2*L+1ppred=ppred+Wc(j)*(Xsigmapre(:,j)-xpred)*(Xsigmapre(:,j)-xpred);endppred=ppred+processerror;ch。

    8、or=(chol(L+ramda)*ppred);for j=1:LXaugsigmaP1(:,j)=xpred+chor(:,j);XaugsigmaP2(:,j)=xpred-chor(:,j);endXaugsigma=xpred XaugsigmaP1 XaugsigmaP2 ; for j=1:2*L+1Ysigmapre(1,j)=atan(Xaugsigma(3,j)/Xaugsigma(1,j) ;Ysigmapre(2,j)=sqrt(Xaugsigma(1,j)2+(Xaugsigma(3,j)2);endypred=zeros(2,1);for j=1:2*L+1 ypr。

    9、ed=ypred+Wm(j)*Ysigmapre(:,j);endPzz=zeros(2,2);for j=1:2*L+1Pzz=Pzz+Wc(j)*(Ysigmapre(:,j)-ypred)*(Ysigmapre(:,j)-ypred);endPzz=Pzz+measureerror;Pxy=zeros(Anoise,2);for j=1:2*L+1Pxy=Pxy+Wc(j)*(Xaugsigma(:,j)-xpred)*(Ysigmapre(:,j)-ypred);endK=Pxy*inv(Pzz);xestimate=xpred+K*(Zmeasure(:,i)-ypred);P=pp。

    10、red-K*Pzz*K; xukf(i)=xestimate(1,1); yukf(i)=xestimate(3,1);% EKF PRO%if i=1ekf_p=xerror xerror/t xyerror xyerror/t;xerror/t 2*xerror/(t2) xyerror/t 2*xyerror/(t2);xyerror xyerror/t yerror yerror/t;xyerror/t 2*xyerror/(t2) yerror/t 2*yerror/(t2);ekf_xestimate=Zmeasure(2,i)*cos(Zmeasure(1,i) 0 Zmeasu。

    11、re(2,i)*sin(Zmeasure(1,i) 0 ;ekf_xpred=ekf_xestimate;end;F=A; ekf_xpred=F*ekf_xestimate;ekf_ppred=F*ekf_p*F+processerror;H=-ekf_xpred(3)/(ekf_xpred(3)2+ekf_xpred(1)2) 0 ekf_xpred(1)/(ekf_xpred(3)2+ekf_xpred(1)2) 0;ekf_xpred(1)/sqrt(ekf_xpred(3)2+ekf_xpred(1)2) 0 ekf_xpred(3)/sqrt(ekf_xpred(3)2+ekf_x。

    12、pred(1)2) 0;ekf_z(1,1)=atan(ekf_xpred(3)/ekf_xpred(1) ;ekf_z(2,1)=sqrt(ekf_xpred(1)2+(ekf_xpred(3)2);PHHP=H*ekf_ppred*H+measureerror;ekf_K=ekf_ppred*H*inv(PHHP);ekf_p=(eye(L)-ekf_K*H)*ekf_ppred; ekf_xestimate=ekf_xpred+ekf_K*(Zmeasure(:,i)-ekf_z); traceekf(i)=trace(ekf_p);xekf(i)=ekf_xestimate(1,1);。

    13、 yekf(i)=ekf_xestimate(3,1); errorx(i)=xx(i)+xradarpositon-x(i);errory(i)=yy(i)+yradarpositon-y(i); ukferrorx(i)=xestimate(1)+xradarpositon-x(i);ukferrory(i)=xestimate(3)+yradarpositon-y(i);ekferrorx(i)=ekf_xestimate(1)+xradarpositon-x(i);ekferrory(i)=ekf_xestimate(3)+yradarpositon-y(i);aa(i)=xx(i)+。

    14、xradarpositon-x(i);bb(i)=yy(i)+yradarpositon-y(i);sumx=sumx+(errorx(i)2);sumy=sumy+(errory(i)2);sumxukf=sumxukf+(ukferrorx(i)2);sumyukf=sumyukf+(ukferrory(i)2); sumxekf=sumxekf+(ekferrorx(i)2);sumyekf=sumyekf+(ekferrory(i)2);mseerrorx(i)=sqrt(sumx/(i-1);%噪声的统计均方误差mseerrory(i)=sqrt(sumy/(i-1);mseerro。

    15、rxukf(i)=sqrt(sumxukf/(i-1);%UKF的统计均方误差mseerroryukf(i)=sqrt(sumyukf/(i-1);mseerrorxekf(i)=sqrt(sumxekf/(i-1);%EKF的统计均方误差mseerroryekf(i)=sqrt(sumyekf/(i-1);endfigure(1);plot(mseerrorxukf,r);hold on;plot(mseerrorxekf,g);hold on;plot(mseerrorx,.);hold on;ylabel(MSE of X axis,fontsize,15);xlabel(sample 。

    16、number,fontsize,15);legend(UKF,EKF,measurement error);figure(2)plot(mseerroryukf,r); hold on;plot(mseerroryekf,g); hold on;plot(mseerrory,.);hold on;ylabel(MSE of Y axis,fontsize,15);xlabel(sample number,fontsize,15);legend(UKF,EKF,measurement error);figure(3)plot(x,y);hold on;plot(xekf,yekf,g);hold on;plot(xukf,yukf,r);hold on;plot(xx,yy,m); ylabel( X ,fontsize,15);xlabel(Y,fontsize,15);legend(TRUE,UKF,EKF,measurements);。

    展开全文
  • 扩展卡尔曼滤波和粒子滤波算法的MATLAB实现
  • matlab写的一个扩展卡尔曼滤波程序,状态方程为线性,观测方程非线性,最后输出图片以便观察是否收敛,分享给大家参考。
  • matlab写的一个扩展卡尔曼滤波程序,状态方程为线性,观测方程非线性,最后输出图片以便观察是否收敛,分享给大家参考。还有一个C++版本的。
  • 扩展卡尔曼滤波MATLAB程序,官方的原版程序,可以参考
  • TDOA AOA定位的扩展卡尔曼滤波定位算法Matlab源码-TDOA AOA positioning of extended Kalman filter algorithm Matlab source
  • 扩展卡尔曼滤波在目标跟踪中的应用 扩展卡尔曼滤波算法; MATLAB仿真 目标跟踪matlab仿真实现; Case: 二维目标跟踪情况 对应的理论分析和参数设置,见博文...
  • 扩展卡尔曼滤波算法的matlab实例,将扩展卡尔曼滤波简介明了的用代码表示,有助于读者了解该算法,对算法有深一步的认识
  • 扩展卡尔曼滤波代码

    2016-03-05 16:02:53
    扩展卡尔曼滤波代码,详细的matlab代码
  • 扩展卡尔曼滤波在三维目标跟踪中的应用 扩展卡尔曼滤波算法; MATLAB仿真 目标跟踪matlab仿真实现; Case: 二维目标跟踪情况 对应的理论分析和参数设置,见博文...
  • 本文使用 Zhihu On VSCode 创作并发布0 前言扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线性情形下的一种扩展形式,它是一种高效率的递归滤波器(自回归滤波器)。EKF 的基本思想是利用泰勒...

    85b9d8cbebd9dc492e5a828fb5e8db38.png
    本文使用 Zhihu On VSCode 创作并发布

    0 前言

    扩展卡尔曼滤波(Extended Kalman Filter,EKF)是标准卡尔曼滤波在非线性情形下的一种扩展形式,它是一种高效率的递归滤波器(自回归滤波器)。

    EKF 的基本思想是利用泰勒级数展开将非线性系统的状态转移函数

    和(或)观测函数
    线性化,然后采用卡尔曼滤波框架对信号进行滤波,因此它是一种次优滤波。

    1 贝叶斯滤波的三大概率密度函数

    在此前的文章

    石鹏:从概率到贝叶斯滤波zhuanlan.zhihu.com
    db1c4f17fef7f14a399157d3686313f4.png

    石鹏:从贝叶斯滤波到卡尔曼滤波zhuanlan.zhihu.com
    fa118278ff078aa71066f6b7d503454a.png

    中,已经讲到贝叶斯滤波的先验概率密度函数、似然概率密度函数和后验概率密度函数:

    (1) 先验概率密度函数

    (2) 似然概率密度函数

    (3) 后验概率密度函数

    其中,后验概率密度函数中的归一化常数

    为:

    2 扩展卡尔曼滤波的假设

    扩展卡尔曼滤波以贝叶斯滤波为理论基础,并作了五个前提假设。

    2.1 与卡尔曼滤波相同的假设

    (1) 假设一:状态量服从正态分布

    (2) 假设二:观测量服从正态分布

    (3) 假设三:过程噪声服从均值为 0 的正态分布

    (4) 假设四:观测噪声服从均值为 0 的正态分布

    2.2 与卡尔曼滤波不同的假设

    (5) 假设五:状态转移函数和(或)观测函数为非线性函数

    在卡尔曼滤波的前提假设中,认为状态方程中的状态转移函数

    以及观测方程中的观测函数
    均为线性函数。基于这种线性假设,存在常数或常矩阵
    ,使得
    可以写成卡尔曼滤波中的线性形式,存在常数或常矩阵
    ,使得
    也可以写成卡尔曼滤波中的线性形式。

    不同于标准卡尔曼滤波,扩展卡尔曼滤波处理的是非线性系统,假设系统的状态转移函数和(或)观测函数为非线性函数。

    3 扩展卡尔曼滤波的公式推导

    3.1 预测步的两个公式

    当状态转移函数为线性函数时,扩展卡尔曼滤波的预测步与标准卡尔曼滤波相同;当状态转移函数为非线性函数时,扩展卡尔曼滤波的预测步采用下面的推导过程。

    根据假设一,

    时刻状态量
    服从均值为
    ,方差为
    的正态分布:

    的后验概率密度函数为:

    对状态转移函数

    的后验估计处
    (即
    )进行
    一阶泰勒级数展开

    其中,

    为高阶无穷小量,对其进行舍弃,便可得到状态转移函数
    的近似形式:

    ,则近似有:

    结合假设三与上式,

    时刻状态量
    的先验概率密度函数为:

    类似推导标准卡尔曼滤波时的做法,使用 Mathematica 软件做符号推导,整理表达式结果可知,先验概率密度函数

    为正态分布函数,均值和方差分别为:

    其中,

    ,对于一维扩展卡尔曼滤波,
    为一常数项,即状态转移函数
    的一阶导函数
    在上一时刻状态量后验估计处的函数值。

    3.2 更新步的三个公式

    当观测函数为线性函数时,扩展卡尔曼滤波的更新步与标准卡尔曼滤波相同;当观测函数为非线性函数时,扩展卡尔曼滤波的更新步采用下面的推导过程。

    对观测函数

    的先验估计处
    (即
    )进行
    一阶泰勒级数展开

    其中,

    为高阶无穷小量,对其进行舍弃,便可得到观测函数
    的近似形式:

    ,则近似有:

    结合假设四、公式 (3.1)、公式 (3.2) 及

    的线性化近似形式,可知,k 时刻状态量
    的后验概率密度函数为:

    其中,归一化常数

    为:

    类似推导标准卡尔曼滤波时的做法,使用 Mathematica 软件做符号推导,整理表达式结果可知,后验概率密度函数

    为正态分布函数,均值和方差分别为:

    即 k 时刻状态量
    的后验估计
    。其中,
    被称为卡尔曼增益系数:

    其中,

    ,对于一维扩展卡尔曼滤波,
    为一常数项,即观测函数
    的一阶导函数
    在当前时刻状态量先验估计处的函数值。

    4 矩阵形式的扩展卡尔曼滤波

    上文内容所描述的是一维的扩展卡尔曼滤波,当状态量和观测量不再是单一的随机变量而是由多个随机变量组成的序列时,扩展卡尔曼滤波中各个量的维数也将随之改变:

    • 状态量
      由随机变量演变为随机向量,随机向量中的每一个分量为一个状态量随机变量。维数为
    • 状态转移比例项
      演变为
      雅可比矩阵,维数为
      。此时,
      是由每个状态转移函数对每个状态量分量求偏导后,将上一时刻状态量的后验估计代入得到:

    • 状态量概率密度函数均值
      演变为矩阵,维数为
    • 状态量概率密度函数方差
      演变为协方差矩阵,用
      表示,维数为
    • 过程噪声方差
      演变为协方差矩阵,用
      表示,维数为
    • 观测量
      由随机变量演变为随机向量,随机向量中的每一个分量为一个观测量随机变量。维数为
    • 观测值
      由单一值演变为由单一值组成的值矩阵,维数为
    • 观测比例项
      演变为
      雅可比矩阵,维数为
      。此时,
      是由每个观测函数对每个观测量分量求偏导后,将当前时刻状态量的先验估计代入得到:

    • 观测噪声方差
      演变为协方差矩阵,用
      表示,维数为
    • 卡尔曼增益系数
      演变为矩阵,维数为

    对应的五个公式演变为:

    公式 (4.3) 中

    即 k 时刻状态量
    的后验估计
    常被称为
    残差(Residual)或新息(Innovation);公式 (4.4) 中的
    代表单位矩阵,维数为

    当然,对于某个非线性系统,不一定状态转移和观测都是非线性的:

    • 线性的状态转移 + 非线性的观测
      此时,滤波递推公式由卡尔曼滤波的预测步两公式和扩展卡尔曼滤波的更新步三公式组成。
    • 非线性的状态转移 + 线性的观测
      此时,滤波递推公式由扩展卡尔曼滤波的预测步两公式和卡尔曼滤波的更新步三公式组成。

    5 应用实例——基于毫米波雷达与扩展卡尔曼滤波的目标跟踪

    5.1 系统分析

    毫米波雷达(Radar)与激光雷达的的检测原理不同。激光雷达利用光的直线传播原理获得目标在笛卡尔坐标系下的距离信息;毫米波雷达利用电磁波的多普勒效应,获得目标在极坐标系下的距离

    、方向角
    和距离变化率(径向速度)

    假设我们使用毫米波雷达对某作匀速直线运动的目标在笛卡尔坐标系内的横坐标

    、纵坐标
    、横向速度
    、纵向速度
    进行跟踪,则状态量可设为:

    观测量可设为:

    故,毫米波雷达的测量数据特性可用下图进行表征。

    270902be8a715ab53e27c2c89237afdc.png
    毫米波雷达的测量数据特性

    其中,距离变化率

    是绝对速度在径向上的投影。利用几何关系与三角函数进行推导,可知:

    故,观测函数

    为:

    显然为非线性函数,而系统的状态转移(CV 模型)又明显是线性的,因此,此处的扩展卡尔曼滤波形式为:线性的状态转移 + 非线性的观测。

    此时,滤波递推公式由卡尔曼滤波的预测步两公式和扩展卡尔曼滤波的更新步三公式组成:

    卡尔曼滤波的预测步两公式

    扩展卡尔曼滤波的更新步三公式

    由于目标作匀速直线运动,故公式 (5.1) 中的控制项

    可以忽略。

    5.2 雅可比矩阵求解

    公式 (5.4) 和公式 (5.5) 中的雅可比矩阵

    根据定义可表示为:

    直接求解

    较为繁琐,可通过 matlab 的内置函数
    jacobian() 进行求解。创建 matlab 脚本,命名为 CalculateHj.m 并输入如下内容后保存:
    % define symbol variables
    syms px py vx vy
    
    % state random vector
    X = [px py vx vy];
    
    % measurement functions
    h1 = sqrt(px^2 + py^2);
    h2 = atan(py / px);
    h3 = (px * vx + py * vy) / sqrt(px^2 + py^2);
    
    % measurement jacobian matrix
    Hj = jacobian([h1 h2 h3], X)
    

    运行 CalculateHj.m 脚本,得到

    如下运算结果:
    Hj =
     
    [                                            px/(px^2 + py^2)^(1/2),                                            py/(px^2 + py^2)^(1/2),                      0,                      0]
    [                                        -py/(px^2*(py^2/px^2 + 1)),                                            1/(px*(py^2/px^2 + 1)),                      0,                      0]
    [ vx/(px^2 + py^2)^(1/2) - (px*(px*vx + py*vy))/(px^2 + py^2)^(3/2), vy/(px^2 + py^2)^(1/2) - (py*(px*vx + py*vy))/(px^2 + py^2)^(3/2), px/(px^2 + py^2)^(1/2), py/(px^2 + py^2)^(1/2)]
    

    可以直接使用上述结果,也可以对其进行化简:

    5.3 代码

    代码与此前的文章

    (十三)手把手教你写卡尔曼滤波器blog.shipengx.com
    e89bf188f3836033a4a79b0e06be6566.png

    相似,区别在于状态量和观测量有所不同,且使用雅可比矩阵

    代替了观测矩阵
    (对于状态转移非线性的系统,还需使用雅可比矩阵
    代替状态转移矩阵
    )。

    ExtendedKalmanFilter.h

    #ifndef EXTENDED_KALMAN_FILTER_
    #define EXTENDED_KALMAN_FILTER_
    
    #include "D:/eigen3/Eigen/Dense"
    
    class ExtendedKalmanFilter
    {
    private:
        bool is_inited;          // flag of initialization
        bool just_begin_filt;    // flag of just begining filt data
        Eigen::VectorXd X;       // state vector
        Eigen::MatrixXd F;       // state transition matrix
        Eigen::MatrixXd P;       // state covariance matrix
        Eigen::MatrixXd Q;       // process noise covariance matrix
        Eigen::MatrixXd Hj;      // measurement jacobian matrix
        Eigen::MatrixXd R;       // measurement noise covariance matrix
        Eigen::MatrixXd K;       // extended kalman gain coefficient
        uint64_t timestamp_last; // timestamp of last frame: us
        float dt;                // delta time: s
    
    public:
        Eigen::VectorXd Z;      // measurement vector
        uint64_t timestamp_now; // timestamp of current frame: us
    
    public:
        ExtendedKalmanFilter();
        ~ExtendedKalmanFilter();
    
        inline bool &IsInited()
        {
            return is_inited;
        }
    
        void Init();
    
        inline void Reset()
        {
            is_inited = false;
        }
    
        inline void SetF(const Eigen::MatrixXd &f)
        {
            F = f;
        }
    
        inline void SetF()
        {
            F << 1.0f, 0.0f, dt, 0.0f,
                0.0f, 1.0f, 0.0f, dt,
                0.0f, 0.0f, 1.0f, 0.0f,
                0.0f, 0.0f, 0.0f, 1.0f;
        }
    
        inline void SetP(const Eigen::MatrixXd &p)
        {
            P = p;
        }
    
        inline void SetQ(const Eigen::MatrixXd &q)
        {
            Q = q;
        }
    
        inline void SetHj(const Eigen::MatrixXd &hj)
        {
            Hj = hj;
        }
    
        inline void SetR(const Eigen::MatrixXd &r)
        {
            R = r;
        }
    
        /**
         * @brief Predict step.
         * 
         */
        void Predict();
    
        /**
         * @brief Update step.
         * 
         */
        void Update();
    };
    
    /**
     * @brief To update measurement vector and timestamp.
     * 
     * @param Z0 Z(0)
     * @param Z1 Z(1)
     * @param Z2 Z(2)
     * @param timestamp Current timestamp.
     */
    void RecvRawData(float &Z0, float &Z1, float &Z2, uint32_t &timestamp)
    {
        // Z0 = rho;
        // Z1 = phi;
        // Z2 = rho_dot;
        // timestamp = timestamp_new;
    }
    
    #endif
    

    ExtendedKalmanFilter.cpp

    #include "math.h"
    
    #include "ExtendedKalmanFilter.h"
    
    /**
     * @brief Construct a new Extended Kalman Filter:: Extended Kalman Filter object
     * 
     */
    ExtendedKalmanFilter::ExtendedKalmanFilter()
        : is_inited(false),
          just_begin_filt(false),
          X(4),
          F(4, 4),
          P(4, 4),
          Q(4, 4),
          Z(3),
          Hj(3, 4),
          R(3, 3),
          K(4, 3),
          timestamp_now(0),
          timestamp_last(0),
          dt(0.0f)
    {
    }
    
    /**
     * @brief Destroy the Extended Kalman Filter:: Extended Kalman Filter object
     * 
     */
    ExtendedKalmanFilter::~ExtendedKalmanFilter() {}
    
    /**
     * @brief Initialize the extended kalman filter.
     * 
     */
    void ExtendedKalmanFilter::Init()
    {
        if (!IsInited())
        {
            X << Z(0) * cos(Z(1)),
                Z(0) * sin(Z(1)),
                Z(2) * cos(Z(1)),
                Z(2) * sin(Z(1));
            P << 1.0, 0.0, 0.0, 0.0,
                0.0, 1.0, 0.0, 0.0,
                0.0, 0.0, 10.0, 0.0,
                0.0, 0.0, 0.0, 10.0;
            Q << 1.0, 0.0, 0.0, 0.0,
                0.0, 1.0, 0.0, 0.0,
                0.0, 0.0, 1.0, 0.0,
                0.0, 0.0, 0.0, 1.0;
            R << 0.09, 0.0, 0.0,
                0.0, 0.009, 0.0,
                0.0, 0.0, 0.09;
            timestamp_last = timestamp_now;
            is_inited = true;
            just_begin_filt = true;
        }
        else
            just_begin_filt = false;
    }
    
    /**
     * @brief Predict step.
     * 
     */
    void ExtendedKalmanFilter::Predict()
    {
        if (just_begin_filt)
            return;
    
        dt = (timestamp_now - timestamp_last) / 1E6;
        SetF();
        timestamp_last = timestamp_now;
    
        // predict state vector
        X = F * X;
    
        // predict state covariance matrix
        P = F * P * F.transpose() + Q;
    }
    
    /**
     * @brief Update step.
     * 
     */
    void ExtendedKalmanFilter::Update()
    {
        if (just_begin_filt)
            return;
    
        float c1 = X(0) ^ 2 + X(1) ^ 2;
        float c2 = sqrt(c1);
        float c3 = c1 * c2;
        if (fabs(c1 > 0.0001))
        {
            Hj << X(0) / c2, X(1) / c2, 0, 0,
                -X(1) / c2, X(0) / c2, 0, 0,
                X(1) * (X(2) * X(1) - X(3) * X(0)) / c3, X(0) * (X(3) * X(0) - X(2) * X(1)) / c3, X(0) / c2, X(1) / c2;
        }
    
        Eigen::VectorXd(3) hx;
        hx << c2, atan2(X(1), X(0)), (X(0) * X(2) + X(1) * X(3)) / c2;
    
        static Eigen::MatrixXd I = Eigen::MatrixXd::Identity(X.size(), X.size());
    
        // calculate extended kalman gain
        K = P * Hj.transpose() * (Hj * P * Hj.transpose() + R).inverse();
    
        // update state vector
        X = X + K * (Z - hx);
    
        // update state covariance matrix
        P = (I - K * Hj) * P;
    }
    

    main.cpp

    #include "ExtendedKalmanFilter.h"
    
    #include <iostream>
    
    int main()
    {
        ExtendedKalmanFilter ekf;
    
        while (1)
        {
            RecvRawData(ekf.Z(0), ekf.Z(1), ekf.Z(2), ekf.timestamp_now);
            Init();
            Predict();
            Update();
        }
    
        return 0;
    }
    

    点击这里下载完整工程。

    6 扩展卡尔曼滤波器的优缺点

    优点

    扩展卡尔曼滤波与标准卡尔曼滤波有着相似的计算形式,因此运算速度同样很快。

    缺点

    扩展卡尔曼滤波对非线性的状态转移函数和(或)观测函数使用一阶泰勒级数展开作了线性近似,忽略了二阶及以上的高阶项,因此精度一般(通常称为一阶精度),对于高度非线性问题效果较差。此外,雅可比矩阵的计算较为繁琐,容易出错。

    总结

    扩展卡尔曼滤波所应用的线性近似是否具有优势主要取决于两个因素:被近似的局部非线性化程度和不确定程度。

    7 参考

    1. b站忠实的王大头《贝叶斯滤波与卡尔曼滤波》第十三讲:扩展卡尔曼滤波及其代码
    2. 无人驾驶技术入门(十八)| 手把手教你写扩展卡尔曼滤波器
    3. 百度百科 - 泰勒公式

    如果觉得文章对你有所帮助的话,欢迎点赞/评论/收藏/关注,相互交流,共同进步~

    展开全文
  • lp__matlab 学习笔记之基于扩展卡尔曼滤波的锂电池SOC估计
  • 注:本文非科普向,会涉及一些一些数学推导,对于具体的算法,也附带了MATLAB代码编程.亲爱的知友们,你们好啊,欢迎来到我的滤波专栏系列鸭~Boya的科研杂记​zhuanlan.zhihu.com我这也是鸽了好久了,最近状态也不太...

    a743e54499c375627295f08e67d71482.png

    注:本文非科普向,会涉及一些一些数学推导,对于具体的算法,也附带了MATLAB代码编程.

    亲爱的知友们,你们好啊,欢迎来到我的滤波专栏系列鸭~

    c07eb1246ed3b1e4606ded7a01b56a7f.png
    Boya的科研杂记zhuanlan.zhihu.com
    ed881dbaa09c5d98573e737b8591a7f7.png

    我这也是鸽了好久了,最近状态也不太好,各种烦心事儿,十分抱歉啊.

    相信大家绝大部分都已经开学了吧,貌似有些学校还是封闭式管理呢,而且部分高校国庆节也才调休3天,打算寒假提前放假.

    不管怎么样,相信大家重新回到学校里面,一定是很开心的吧,我的话,返校的感觉就是学校食堂涨价了,但是量却没涨多少.每顿饭量也挺多的,按照这个趋势下去,我不胖不行啊,嘻嘻(●'◡'●)

    而且我也很感谢大家的支持啊,知乎小白粉丝突破了3K,以后我也会更加频繁地更新相关内容的,大家敬请期待,爱你们哟~(^U^)ノ~YO


    今天我们要讲的内容是非线性卡尔曼滤波的第一种,也是很常见的一种,跟往常一样,我们优先介绍理论,会在接下来的文章中进行数值仿真~

    若大家想了解之前写过的卡尔曼滤波以及仿真,传送通道在这里了:

    Rainsley:贝叶斯滤波(Bayesian filtering)zhuanlan.zhihu.com
    0642552283b5ffe378c03ef5bba38e02.png
    Rainsley:卡尔曼滤波(Kalman filter)zhuanlan.zhihu.com
    36b5037fad504abf6aa9f29dcbd56d09.png
    Rainsley:卡尔曼滤波(Kalman filter)实例研究——汽车跟踪问题(Car tracking)zhuanlan.zhihu.com
    1cd71c4b66cce01884ab609873d187e9.png

    我们就开始今天的内容吧!我们对于卡尔曼滤波,说它其实就是一个线性高斯系统,这里的线性性主要是指运动模型与观测模型均是线性的。

    当然,我们大家一定具备这样的数学思维:数学中的线性的大部分都是,易于处理的,而比较困难的大头的东西是非线性的,往往可以在局部对它进行线性展开近似处理,我们今天要讲的扩展卡尔曼滤波(EKF)就是如此:

    EKF的主要思想——泰勒展开

    我们已经知道非线性滤波主要体现在运动模型以及观测模型都是非线性的,比如:

    emmmm,刚刚去刷了一会儿微博,因为今夜凌晨有Apple的发布会,哈哈哈,我们继续鸭~

    主要思路呢,其实就是对非线性这部分进行近似线性处理,这里我们考虑最一般的泰勒展开:

    到了这里呢,还不够,我们大家要明确一个知识点,就是关于正态分布,我们要得到边缘密度,条件密度,必须已知联合分布,这个是关键的~

    大部分情况下,我们的一个正态随机变量经过非线性变换以后,大概率得到的随机变量不服从正态分布,但是我们进行了一阶线性近似,所以就考虑假设变换后的随机变量仍旧是正态分布.

    过程推导

    有了上面的内容,意味着我们可以对一个随机变量进行非线性变换,然后接着可以得到变换后的随机变量的期望和方差。然后我们就可以直接利用前面的卡尔曼滤波方程,直接进行替代即可.

    所以形式上我们可以看到,卡尔曼滤波之所以够经典,就是因为其他有些滤波完全借助它的框架而进行展开,从EKF的主要思想上我们也不难看出,由于只采用了一阶近似,在后续的迭代中,误差可能会越来越大.这也算是它的缺点了.

    好的,各位小伙伴们!本期滤波内容就到这里了,我们下期再见!

    ☆⌒(*^-゜)v THX!!

    如果觉得我的文章还有用的话,还烦请各位阿娜答三联支持一波哦!

    主要参考资料:《Bayesian Filtering and Smoothing》,Simo Sarkka著。

    展开全文
  • 扩展卡尔曼滤波算法的matlab程序clear allv=150; %%目标速度v_sensor=0;%%传感器速度t=1; %%扫描周期xradarpositon=0; %%传感器坐标yradarpositon=0; %%ppred=zeros(4,4);Pzz=zeros(2,2);Pxx=zeros(4,2);xpred=zeros...
  • %滤波周期 T_go=10;%滤波时间 N=T_go/T;%观测次数 t=0:T:T_go-T;%假定输出序列(供画图用) x=zeros(2,N); z=zeros(2,N); x(:,1)=[1;0];%真值初始值 mu=[0;0]; Q=[0.01,0;0,0.0001]; R=[0.1,0;0,0.1]; rng(1); w=...

空空如也

空空如也

1 2 3 4 5 ... 7
收藏数 139
精华内容 55
关键字:

扩展卡尔曼滤波matlab

matlab 订阅