精华内容
下载资源
问答
  • EKF matlab

    千次阅读 2017-11-11 22:00:35
    但在实际工作中,我们的系统几乎都是非线性的,所以如何解决非线性系统的滤波问题呢,这就是我们要讲的EKF(扩展卡尔曼滤波),它的原理很简单,就是在估计状态的地方进行线性化,线性化的方法也很简单,只需要进行...
    卡尔曼是真的强,我只能这么说,前面说了卡尔曼滤波,它是针对线性系统的滤波方法。但在实际工作中,我们的系统几乎都是非线性的,所以如何解决非线性系统的滤波问题呢,这就是我们要讲的EKF(扩展卡尔曼滤波),它的原理很简单,就是在估计状态的地方进行线性化,线性化的方法也很简单,只需要进行泰勒的一阶展开就行。当然我们也要说一下缺点,因为我们选择的线性化方法,所以只能达到二阶的精度(UKF可以达到四阶的精度),所以要求我们的系统是弱线性化的。其实UKF也是对非线性系统的卡尔曼滤波,主要本人对其线性化方法(UT变换)不是很理解,等考完试再说。
     
    首先,系统的状态方程和观测方程如下:
    {\textbf {x}}_{{k}}=f({\textbf {x}}_{{k-1}},{\textbf {u}}_{{k}},{\textbf {w}}_{{k}})
    {\textbf {z}}_{{k}}=h({\textbf {x}}_{{k}},{\textbf {v}}_{{k}})
    我们知道,在更新误差协方差矩阵的时候,不能直接用f和h的,所以我们要进行泰勒展开,也就是要求雅克比矩阵。再利用线性情况下的卡尔曼滤波进行计算更新。
    预测:
    {\hat {{\textbf {x}}}}_{{k|k-1}}=f({\textbf {x}}_{{k-1}},{\textbf {u}}_{{k}},0)
    {\textbf {P}}_{{k|k-1}}={\textbf {F}}_{{k}}{\textbf {P}}_{{k-1|k-1}}{\textbf {F}}_{{k}}^{{T}}+{\textbf {Q}}_{{k}}
    利用雅克比矩阵进行更新模型:

    {\textbf {F}}_{{k}}=\left.{\frac {\partial f}{\partial {\textbf {x}}}}\right\vert _{{{\hat {{\textbf {x}}}}_{{k-1|k-1}},{\textbf {u}}_{{k}}}}
    {\textbf {H}}_{{k}}=\left.{\frac {\partial h}{\partial {\textbf {x}}}}\right\vert _{{{\hat {{\textbf {x}}}}_{{k|k-1}}}}
    更新:
    {\tilde {{\textbf {y}}}}_{{k}}={\textbf {z}}_{{k}}-h({\hat {{\textbf {x}}}}_{{k|k-1}},0)
    {\textbf {S}}_{{k}}={\textbf {H}}_{{k}}{\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}+{\textbf {R}}_{{k}}
    {\textbf {K}}_{{k}}={\textbf {P}}_{{k|k-1}}{\textbf {H}}_{{k}}^{{T}}{\textbf {S}}_{{k}}^{{-1}}
    {\hat {{\textbf {x}}}}_{{k|k}}={\hat {{\textbf {x}}}}_{{k|k-1}}+{\textbf {K}}_{{k}}{\tilde {{\textbf {y}}}}_{{k}}
    {\textbf {P}}_{{k|k}}=(I-{\textbf {K}}_{{k}}{\textbf {H}}_{{k}}){\textbf {P}}_{{k|k-1}}
    下面我会展示一个目标追踪的EKF例子:
    12/05/16 01:22:15 /home/fc/桌面/EKF/EKF.m
      1 
    %扩展Kalman滤波在目标跟踪中的应用
      2 
    %functionEKF_For_TargetTracking
      3 
    clc;clear;
      4 
    T=1;%雷达扫描周期
      5 
    N=60/T;%总的采样次数
      6 
    X=zeros(4,N);%目标真实位置、速度
      7 
    X(:,1)=[-100,2,200,20];%目标初始位置、速度
      8 
    Z=zeros(1,N);%传感器对位置的观测
      9 
    delta_w=1e-3;%如果增大这个参数,目标的真实轨迹就是曲线了
     10 
    Q=delta_w*diag([0.5,1]);%过程噪声方差
     11 
    G=[T^2/2,0;T,0;0,T^2/2;0,T];%过程噪声驱动矩阵
     12 
    R=5;%观测噪声方差
     13 
    F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态转移矩阵
     14 
    x0=200;%观测站的位置
     15 
    y0=300;
     16 
    Xstation=[x0,y0];
     17 
    for t=2:N
     18 
       X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);%目标真实轨迹
     19 
    end
     20 
     
     21 
    for t=1:N
     22 
       Z(t)=Dist(X(:,t),Xstation)+sqrtm(R)*randn;%观测值
     23 
    end
     24 
    %EKF
     25 
    Xekf=zeros(4,N);
     26 
    Xekf(:,1)=X(:,1);%Kalman滤波的状态初始化
     27 
    P0=eye(4);%误差协方差矩阵的初始化
     28 
    for i=2:N
     29 
       Xn=F*Xekf(:,i-1);%一步预测
     30 
       P1=F*P0*F'+G*Q*G';%预测误差协方差
     31 
       dd=Dist(Xn,Xstation);%观测预测
     32 
       %求解雅克比矩阵H
     33 
       H=[(Xn(1,1)-x0)/dd,0,(Xn(3,1)-y0)/dd,0];%泰勒展开的一阶近似
     34 
       K=P1*H'*inv(H*P1*H'+R);%卡尔曼最优增益
     35 
       Xekf(:,i)=Xn+K*(Z(:,i)-dd);%状态更新
     36 
       P0=(eye(4)-K*H)*P1;%滤波误差协方差更新
     37 
    end
     38 
    %误差分析
     39 
    for i=1:N
     40 
       Err_KalmanFilter(i)=Dist(X(:,i),Xekf(:,i));%
     41 
    end
     42 
    %画图
     43 
    figure
     44 
    hold on;boxon;
     45 
    plot(X(1,:),X(3,:),'-k');%真实轨迹
     46 
    plot(Xekf(1,:),Xekf(3,:),'-r');%扩展Kalman滤波轨迹
     47 
    legend('real trajectory','EKFtrajectory');
     48 
    xlabel('X-axis X/m');
     49 
    ylabel('Y-axisY/m');
     50 
     
     51 
    figure
     52 
    hold on ;boxon;
     53 
    plot(Err_KalmanFilter,'-ks','MarkerFace','r')
     54 
    xlabel('Time/s');
     55 
    ylabel('Positionestimation bias  /m');
     56 
     
     57 
    %子函数求欧氏距离
     58 
    function d=Dist(X1,X2);
     59 
    if length(X2)<=2
     60 
       d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
     61 
    else
     62 
       d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
     63 
    end
     64 
     
     65 
    %子函数求欧氏距离
     66 
    function d=Dist(X1,X2);
     67 
    if length(X2)<=2
     68 
       d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
     69 
    else
     70 
       d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
     71 
    end
     72 
     
     73 
     
     74 
     
     75 
     
     76 
     
     77 
     
     78 
     
    结果如下:
    扩展卡尔曼滤波EKF及其MATLAB实现

    扩展卡尔曼滤波EKF及其MATLAB实现

    展开全文
  • ekf matlab仿真

    2012-06-28 09:45:57
    ekf matlab 关于efk仿真,matlab源代码
  • PX4 EKF MATLAB代码

    2019-01-09 15:36:49
    PX4 EKF MATLAB代码参考,建立以角速度,角加速度,重力加速度,磁通量共12阶的数据为状态的状态方程。以角速度、加速度、磁通量建立量测方程进行EKF滤波
  • EKF MATLAB函数

    2015-04-13 16:13:23
    不要写EKF代码,只要输入系统方程和量测方程即可滤波
  • EKF matlab工具

    2009-08-13 19:14:26
    扩展卡尔曼滤波简介, EKFmatlab例程
  • EKF MATLAB程序

    热门讨论 2011-04-17 12:31:32
    简单的扩展卡尔曼滤波程序,适合于刚刚接触 MATLAB编程的同学参考一下
  • KF+EKF matlab程序实现

    2018-06-22 21:29:09
    主要关于卡尔曼滤波与扩展卡尔曼滤波,包含两个KF程序、一个EKF程序
  • UKF KF EKF MATLAB仿真

    2012-10-18 16:39:11
    介绍了卡尔曼滤波及各种改进算法,包括EKF,UKF等,并给出matlab仿真
  • EKF SLAM Matlab 代码

    2018-06-04 03:08:42
    a simple but elegant lidar based EKF SLAM Matlab code
  • EKF_matlab

    2013-10-15 13:21:28
    具体说明请见 http://blog.csdn.net/apsvvfb/article/details/12651999
  • ekf滤波MATLAB仿真程序

    2011-04-15 15:14:08
    MATLAB仿真程序 我试过 可以用。。。
  • EKF SLAM Matlab仿真实践详解(附源码) 详细内容及源码请移步https://github.com/Nrusher/EKF_SLAM,希望此份报告及源码对您有所帮助。

    EKF SLAM Matlab仿真实践详解(附源码)

    为提供更好的阅读体验,详细内容及源码请移步https://github.com/Nrusher/EKF_SLAM 或 https://gitee.com/nrush/EKF_SLAM,下载PDF报告进行阅读,希望此份报告及源码对您有所帮助。

    展开全文
  • PIX4源码EKFMATLAB代码实现

    千次阅读 2018-03-31 20:59:17
    这个代码是从官网中下载下来的,其中还需要几个函数的代码也在这里可以找到 % IMPORTANT - This script requires the Matlab symbolic toolbox and takes ~3 ...% Derivation of Navigation EKF using a loca...

    这个代码是从官网中下载下来的,其中还需要几个函数的代码也在这里可以找到

    % IMPORTANT - This script requires the Matlab symbolic toolbox and takes ~3 hours to run
    %这个脚本需要MATLAB符号工具箱运行三个小时!!!
    
    % Derivation of Navigation EKF using a local NED earth Tangent Frame and 
    % XYZ body fixed frame
    % Sequential fusion of velocity and position measurements
    % Fusion of true airspeed
    % Sequential fusion of magnetic flux measurements
    % 24 state architecture.
    % IMU data is assumed to arrive at a constant rate with a time step of dt
    % IMU delta angle and velocity data are used as control inputs,
    % not observations
    
    % Author:  Paul Riseborough
    
    % State vector:状态向量
    % attitude quaternion
    % Velocity - m/sec (North, East, Down)
    % Position - m (North, East, Down)
    % Delta Angle bias - rad (X,Y,Z)
    % Delta Velocity bias - m/s (X,Y,Z)
    % Earth Magnetic Field Vector - (North, East, Down)
    % Body Magnetic Field Vector - (X,Y,Z)
    % Wind Vector  - m/sec (North,East)
    
    % Observations:观测量
    % NED velocity - m/s
    % NED position - m
    % True airspeed - m/s
    % angle of sideslip - rad
    % XYZ magnetic flux
    
    % Time varying parameters:
    % XYZ delta angle measurements in body axes - rad
    % XYZ delta velocity measurements in body axes - m/sec
    
    
    %% define symbolic variables and constants定义符号变量和常量
    clear all;
    reset(symengine);
    syms dax day daz real % IMU delta angle measurements in body axes - rad
    syms dvx dvy dvz real % IMU delta velocity measurements in body axes - m/sec
    syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
    syms vn ve vd real % NED velocity - m/sec
    syms pn pe pd real % NED position - m
    syms dax_b day_b daz_b real % delta angle bias - rad
    syms dvx_b dvy_b dvz_b real % delta velocity bias - m/sec
    syms dt real % IMU time step - sec
    syms gravity real % gravity  - m/sec^2重力加速度
    syms daxVar dayVar dazVar dvxVar dvyVar dvzVar real; % IMU delta angle and delta velocity measurement variances
    syms vwn vwe real; % NE wind velocity - m/sec风速
    syms magX magY magZ real; % XYZ body fixed magnetic field measurements - milligauss机体磁场
    syms magN magE magD real; % NED earth fixed magnetic field components - milligauss地磁场
    syms R_VN R_VE R_VD real % variances for NED velocity measurements - (m/sec)^2大地系中速度的方差
    syms R_PN R_PE R_PD real % variances for NED position measurements - m^2大地系中位置测量的方差
    syms R_TAS real  % variance for true airspeed measurement - (m/sec)^2真空速测量的方差
    syms R_MAG real  % variance for magnetic flux measurements - milligauss^2
    syms R_BETA real % variance of sidelsip measurements rad^2侧滑角测量的方差
    syms R_LOS real % variance of LOS angular rate mesurements (rad/sec)^2
    syms ptd real % location of terrain in D axis
    syms decl real; % earth magnetic field declination from true north磁偏角
    syms R_DECL R_YAW real; % variance of declination or yaw angle observation磁偏角和偏航角观测的方差
    syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y  body axes在x轴和y轴上的风相对运动的弹道系数的倒数
    syms rho real % air density (kg/m^3)大气密度
    syms R_ACC real % variance of accelerometer measurements (m/s^2)^2加速度测量的方差
    syms Kaccx Kaccy real % derivative of X and Y body specific forces wrt componenent of true airspeed along each axis (1/s)真空速沿各轴的X和Y体比力wrt分量的导数
    
    %% define the state prediction equations定义状态预测方程
    
    % define the measured Delta angle and delta velocity vectors
    dAngMeas = [dax; day; daz];
    dVelMeas = [dvx; dvy; dvz];
    
    % define the IMU bias errors and scale factor
    dAngBias = [dax_b; day_b; daz_b];
    dVelBias = [dvx_b; dvy_b; dvz_b];
    
    % define the quaternion rotation vector for the state estimate
    quat = [q0;q1;q2;q3];
    % derive the truth body to nav direction cosine matrix推导机体系到导航坐标系的余弦矩阵
    Tbn = Quat2Tbn(quat);
    
    % define the truth delta angle定义真实角度增量,忽略圆锥振动补偿
    % ignore coning compensation as these effects are negligible in terms of 
    % covariance growth for our application and grade of sensor
    dAngTruth = dAngMeas - dAngBias;
    
    % Define the truth delta velocity -ignore sculling and transport rate
    % corrections as these negligible are in terms of covariance growth for our
    % application and grade of sensor
    dVelTruth = dVelMeas - dVelBias;
    
    % define the attitude update equations定义姿态更新方程
    % use a first order expansion of rotation to calculate the quaternion increment
    % acceptable for propagation of covariances
    deltaQuat = [1;
        0.5*dAngTruth(1);
        0.5*dAngTruth(2);
        0.5*dAngTruth(3);
        ];
    quatNew = QuatMult(quat,deltaQuat);
    
    % define the velocity update equations定义速度更新方程
    % ignore coriolis terms for linearisation purposes因为线性化的目的忽略科里奥利力
    vNew = [vn;ve;vd] + [0;0;gravity]*dt + Tbn*dVelTruth;
    
    % define the position update equations定义位置更新方程
    pNew = [pn;pe;pd] + [vn;ve;vd]*dt;
    
    % define the IMU error update equations定义IMU误差更新方程
    dAngBiasNew = dAngBias;
    dVelBiasNew = dVelBias;
    
    % define the wind velocity update equations定义风速更新方程
    vwnNew = vwn;
    vweNew = vwe;
    
    % define the earth magnetic field update equations定义地磁场更新方程
    magNnew = magN;
    magEnew = magE;
    magDnew = magD;
    
    % define the body magnetic field update equations定义机体磁场更新方程
    magXnew = magX;
    magYnew = magY;
    magZnew = magZ;
    
    % Define the state vector & number of states定义状态矢量和状态个数
    stateVector = [quat;vn;ve;vd;pn;pe;pd;dAngBias;dVelBias;magN;magE;magD;magX;magY;magZ;vwn;vwe];
    nStates=numel(stateVector);
    
    % Define vector of process equations定义状态方程矢量
    newStateVector = [quatNew;vNew;pNew;dAngBiasNew;dVelBiasNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew];
    
    % derive the state transition matrix推导状态转移矩阵
    F = jacobian(newStateVector, stateVector);
    % set the rotation error states to zero
    [F,SF]=OptimiseAlgebra(F,'SF');
    
    % define a symbolic covariance matrix using strings to represent 定义符号协方差矩阵使用字符串表示
    % '_l_' to represent '( '
    % '_c_' to represent ,
    % '_r_' to represent ')' 
    % these can be substituted later to create executable code
    for rowIndex = 1:nStates
        for colIndex = 1:nStates
            eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']);
            eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']);
        end
    end
    
    save 'StatePrediction.mat';
    
    %% derive the covariance prediction equations推导协方差矩阵
    % This reduces the number of floating point operations by a factor of 6 or这使得浮点运算的数量减少了6倍或更多,比起代码中使用标准的矩阵运算。
    % more compared to using the standard matrix operations in code
    
    % Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and
    % velocities, after bias effects have been removed. 
    %惯性解决方案中的误差增长被认为是由角度和速度的增量“噪声”所驱动的,在偏差影响被消除之后。
    
    % derive the control(disturbance) influence matrix from IMu noise to state从IMU噪声到状态的控制(扰动)影响矩阵
    % noise
    G = jacobian(newStateVector, [dAngMeas;dVelMeas]);
    [G,SG]=OptimiseAlgebra(G,'SG');
    
    % derive the state error matrix推导出状态误差矩阵
    distMatrix = diag([daxVar dayVar dazVar dvxVar dvyVar dvzVar]);
    Q = G*distMatrix*transpose(G);
    [Q,SQ]=OptimiseAlgebra(Q,'SQ');
    
    % Derive the predicted covariance matrix using the standard equation利用标准方程推导出预测的协方差矩阵
    PP = F*P*transpose(F) + Q;
    
    % Collect common expressions to optimise processing收集常用表达式来优化处理
    [PP,SPP]=OptimiseAlgebra(PP,'SPP');
    
    save('StateAndCovariancePrediction.mat');%状态和协方差矩阵预测
    clear all;
    reset(symengine);
    
    %% derive equations for fusion of true airspeed measurements推导真航速融合的方程
    load('StatePrediction.mat');
    VtasPred = sqrt((vn-vwn)^2 + (ve-vwe)^2 + vd^2); % predicted measurement
    H_TAS = jacobian(VtasPred,stateVector); % measurement Jacobian观测矩阵雅克比
    [H_TAS,SH_TAS]=OptimiseAlgebra(H_TAS,'SH_TAS'); % optimise processing优化处理
    K_TAS = (P*transpose(H_TAS))/(H_TAS*P*transpose(H_TAS) + R_TAS);%求解卡尔曼增益,R_TAS真空速测量的方差
    [K_TAS,SK_TAS]=OptimiseAlgebra(K_TAS,'SK_TAS'); % Kalman gain vector卡尔曼增益矢量
    
    % save equations and reset workspace保存方程重置工作空间
    save('Airspeed.mat','SH_TAS','H_TAS','SK_TAS','K_TAS');
    clear all;
    reset(symengine);
    
    
    %% derive equations for fusion of angle of sideslip measurements推导侧滑角测量的融合方程
    load('StatePrediction.mat');
    
    % calculate wind relative velocities in nav frame and rotate into body frame在导航坐标系中推导相对风速转换到机体坐标系
    Vbw = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd];
    % calculate predicted angle of sideslip using small angle assumption利用小角度假设,计算侧滑角的预测角?
    BetaPred = Vbw(2)/Vbw(1);
    H_BETA = jacobian(BetaPred,stateVector); % measurement Jacobian
    [H_BETA,SH_BETA]=OptimiseAlgebra(H_BETA,'SH_BETA'); % optimise processing
    K_BETA = (P*transpose(H_BETA))/(H_BETA*P*transpose(H_BETA) + R_BETA);[K_BETA,SK_BETA]=OptimiseAlgebra(K_BETA,'SK_BETA'); % Kalman gain vector
    
    % save equations and reset workspace
    save('Sideslip.mat','SH_BETA','H_BETA','SK_BETA','K_BETA');
    clear all;
    reset(symengine);
    
    %% derive equations for fusion of magnetic field measurement推导磁场融合的方程
    load('StatePrediction.mat');
    
    magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; % predicted measurement
    H_MAG = jacobian(magMeas,stateVector); % measurement Jacobian计算雅克比矩阵
    [H_MAG,SH_MAG]=OptimiseAlgebra(H_MAG,'SH_MAG');
    
    %分开计算磁力计三轴卡尔曼增益是因为磁力计融合模式的不同
    K_MX = (P*transpose(H_MAG(1,:)))/(H_MAG(1,:)*P*transpose(H_MAG(1,:)) + R_MAG); % Kalman gain vector
    [K_MX,SK_MX]=OptimiseAlgebra(K_MX,'SK_MX');
    K_MY = (P*transpose(H_MAG(2,:)))/(H_MAG(2,:)*P*transpose(H_MAG(2,:)) + R_MAG); % Kalman gain vector
    [K_MY,SK_MY]=OptimiseAlgebra(K_MY,'SK_MY');
    K_MZ = (P*transpose(H_MAG(3,:)))/(H_MAG(3,:)*P*transpose(H_MAG(3,:)) + R_MAG); % Kalman gain vector
    [K_MZ,SK_MZ]=OptimiseAlgebra(K_MZ,'SK_MZ');
    
    % save equations and reset workspace
    save('Magnetometer.mat','SH_MAG','H_MAG','SK_MX','K_MX','SK_MY','K_MY','SK_MZ','K_MZ');
    clear all;
    reset(symengine);
    
    
    %% derive equations for sequential fusion of optical flow measurements推导光流融合方程
    load('StatePrediction.mat');
    
    % Range is defined as distance from camera focal point to object measured
    % along sensor Z axis距离定义为从摄像机焦点到测量沿传感器Z轴的物体的距离
    syms range real;
    
    % Define rotation matrix from body to sensor frame定义从机体到传感器的旋转矩阵
    syms Tbs_a_x Tbs_a_y Tbs_a_z real;
    syms Tbs_b_x Tbs_b_y Tbs_b_z real;
    syms Tbs_c_x Tbs_c_y Tbs_c_z real;
    Tbs = [ ...
        Tbs_a_x Tbs_a_y Tbs_a_z ; ...
        Tbs_b_x Tbs_b_y Tbs_b_z ; ...
        Tbs_c_x Tbs_c_y Tbs_c_z ...
        ];
    
    % Calculate earth relative velocity in a non-rotating sensor frame在非旋转的传感器框架中计算地球相对速度
    relVelSensor = Tbs * transpose(Tbn) * [vn;ve;vd];
    
    % Divide by range to get predicted angular LOS rates relative to X and Y
    % axes. Note these are rates in a non-rotating sensor frame
    %除以距离来得到预测的角速度相对于X轴和Y轴。注意这些是在一个非旋转的传感器框架的速率
    losRateSensorX = +relVelSensor(2)/range;
    losRateSensorY = -relVelSensor(1)/range;
    
    save('temp1.mat','losRateSensorX','losRateSensorY');
    
    clear all;
    reset(symengine);
    load('StatePrediction.mat');
    load('temp1.mat');
    
    % calculate the observation Jacobian and Kalman gain for the X axis计算X轴观测雅克比矩阵和卡尔曼系数
    H_LOSX = jacobian(losRateSensorX,stateVector); % measurement Jacobian
    H_LOSX = simplify(H_LOSX);
    K_LOSX = (P*transpose(H_LOSX))/(H_LOSX*P*transpose(H_LOSX) + R_LOS); % Kalman gain vector
    K_LOSX = simplify(K_LOSX);
    save('temp2.mat','H_LOSX','K_LOSX');
    ccode([H_LOSX;transpose(K_LOSX)],'file','LOSX.c');
    fix_c_code('LOSX.c');
    
    clear all;
    reset(symengine);
    load('StatePrediction.mat');
    load('temp1.mat');
    
    % calculate the observation Jacobian for the Y axis计算Y轴的观测雅克比矩阵
    H_LOSY = jacobian(losRateSensorY,stateVector); % measurement Jacobian
    H_LOSY = simplify(H_LOSY);
    K_LOSY = (P*transpose(H_LOSY))/(H_LOSY*P*transpose(H_LOSY) + R_LOS); % Kalman gain vector卡尔曼增益矢量
    K_LOSY = simplify(K_LOSY);
    save('temp3.mat','H_LOSY','K_LOSY');
    ccode([H_LOSY;transpose(K_LOSY)],'file','LOSY.c');
    fix_c_code('LOSY.c');
    
    % reset workspace
    clear all;
    reset(symengine);
    
    
    %% derive equations for sequential fusion of body frame velocity measurements推导机体轴系速度的融合
    load('StatePrediction.mat');
    
    % body frame velocity observations
    syms velX velY velZ real;
    
    % velocity observation variance
    syms R_VEL real;
    
    % calculate relative velocity in body frame计算机体系中的相对速度(视觉里程计)
    relVelBody = transpose(Tbn)*[vn;ve;vd];
    
    save('temp1.mat','relVelBody','R_VEL');
    
    % calculate the observation Jacobian for the X axis计算X轴观测雅克比矩阵
    H_VELX = jacobian(relVelBody(1),stateVector); % measurement Jacobian
    H_VELX = simplify(H_VELX);
    save('temp2.mat','H_VELX');
    ccode(H_VELX,'file','H_VELX.c');
    fix_c_code('H_VELX.c');
    
    clear all;
    reset(symengine);
    load('StatePrediction.mat');
    load('temp1.mat');
    
    % calculate the observation Jacobian for the Y axis计算Y轴速度的观测雅克比矩阵
    H_VELY = jacobian(relVelBody(2),stateVector); % measurement Jacobian
    H_VELY = simplify(H_VELY);
    save('temp3.mat','H_VELY');
    ccode(H_VELY,'file','H_VELY.c');
    fix_c_code('H_VELY.c');
    
    clear all;
    reset(symengine);
    load('StatePrediction.mat');
    load('temp1.mat');
    
    % calculate the observation Jacobian for the Z axis计算Z轴速度观测的雅克比矩阵
    H_VELZ = jacobian(relVelBody(3),stateVector); % measurement Jacobian
    H_VELZ = simplify(H_VELZ);
    save('temp4.mat','H_VELZ');
    ccode(H_VELZ,'file','H_VELZ.c');
    fix_c_code('H_VELZ.c');
    
    clear all;
    reset(symengine);
    
    % calculate Kalman gain vector for the X axis计算卡尔曼增益
    load('StatePrediction.mat');
    load('temp1.mat');
    load('temp2.mat');
    
    K_VELX = (P*transpose(H_VELX))/(H_VELX*P*transpose(H_VELX) + R_VEL); % Kalman gain vector
    K_VELX = simplify(K_VELX);
    ccode(K_VELX,'file','K_VELX.c');
    fix_c_code('K_VELX.c');
    
    clear all;
    reset(symengine);
    
    % calculate Kalman gain vector for the Y axis
    load('StatePrediction.mat');
    load('temp1.mat');
    load('temp3.mat');
    
    K_VELY = (P*transpose(H_VELY))/(H_VELY*P*transpose(H_VELY) + R_VEL); % Kalman gain vector
    K_VELY = simplify(K_VELY);
    ccode(K_VELY,'file','K_VELY.c');
    fix_c_code('K_VELY.c');
    
    clear all;
    reset(symengine);
    
    % calculate Kalman gain vector for the Z axis
    load('StatePrediction.mat');
    load('temp1.mat');
    load('temp4.mat');
    
    K_VELZ = (P*transpose(H_VELZ))/(H_VELZ*P*transpose(H_VELZ) + R_VEL); % Kalman gain vector
    K_VELZ = simplify(K_VELZ);
    ccode(K_VELZ,'file','K_VELZ.c');
    fix_c_code('K_VELZ.c');
    
    % reset workspace
    clear all;
    reset(symengine);
    
    % calculate Kalman gains vectors for X,Y,Z to take advantage of common
    % terms
    load('StatePrediction.mat');
    load('temp1.mat');
    load('temp2.mat');
    load('temp3.mat');
    load('temp4.mat');
    K_VELX = (P*transpose(H_VELX))/(H_VELX*P*transpose(H_VELX) + R_VEL); % Kalman gain vector
    K_VELY = (P*transpose(H_VELY))/(H_VELY*P*transpose(H_VELY) + R_VEL); % Kalman gain vector
    K_VELZ = (P*transpose(H_VELZ))/(H_VELZ*P*transpose(H_VELZ) + R_VEL); % Kalman gain vector
    K_VEL = simplify([K_VELX,K_VELY,K_VELZ]);%化简方程式
    ccode(K_VEL,'file','K_VEL.c');
    fix_c_code('K_VEL.c');
    
    
    %% derive equations for fusion of 321 sequence yaw measurement推导欧拉角321序列偏航角测量的融合方程
    load('StatePrediction.mat');
    
    % Calculate the yaw (first rotation) angle from the 321 rotation sequence计算偏航角测量角度
    angMeas = atan(Tbn(2,1)/Tbn(1,1));
    H_YAW321 = jacobian(angMeas,stateVector); % measurement Jacobian
    H_YAW321 = simplify(H_YAW321);
    ccode(H_YAW321,'file','calcH_YAW321.c');
    fix_c_code('calcH_YAW321.c');
    
    % reset workspace
    clear all;
    reset(symengine);
    
    %% derive equations for fusion of 312 sequence yaw measurement推导欧拉角312序列偏航角测量角度
    load('StatePrediction.mat');
    
    % Calculate the yaw (first rotation) angle from an Euler 312 sequence
    angMeas = atan(-Tbn(1,2)/Tbn(2,2));
    H_YAW312 = jacobian(angMeas,stateVector); % measurement Jacobianclea
    H_YAW312 = simplify(H_YAW312);
    ccode(H_YAW312,'file','calcH_YAW312.c');
    fix_c_code('calcH_YAW312.c');
    
    % reset workspace
    clear all;
    reset(symengine);
    
    
    %% derive equations for fusion of declination推导磁偏角融合方程
    load('StatePrediction.mat');
    
    % the predicted measurement is the angle wrt magnetic north of the horizontal
    % component of the measured field
    angMeas = atan(magE/magN);%计算磁偏角
    H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian观测雅克比矩阵
    H_MAGD = simplify(H_MAGD);
    K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL);
    K_MAGD = simplify(K_MAGD);
    ccode([K_MAGD,H_MAGD'],'file','calcMAGD.c');
    fix_c_code('calcMAGD.c');
    
    % reset workspace
    clear all;
    reset(symengine);
    
    %% derive equations for fusion of lateral body acceleration (multirotors only)对于多旋翼推导了横向加速度的融合方程
    load('StatePrediction.mat');
    
    % use relationship between airspeed along the X and Y body axis and the
    % drag to predict the lateral acceleration for a multirotor vehicle type
    % where propulsion forces are generated primarily along the Z body axis
    %利用X轴和Y轴的飞行速度和阻力来预测多旋翼飞行器的侧向加速度,在此情况下,推进力主要沿Z轴方向产生
    vrel = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; % predicted wind relative velocity
    
    % calculate drag assuming flight along axis in positive direction
    % sign change will be looked after in implementation rather than by adding
    % sign functions to symbolic derivation which genererates output with dirac
    % functions
    %计算阻力沿轴向正方向的变化将在执行过程中进行,而不是通过在符号推导中增加符号函数,从而使输出具有狄拉克函数delta函数
    % accXpred = -0.5*rho*vrel(1)*vrel(1)*BCXinv; % predicted acceleration measured along X body axis
    % accYpred = -0.5*rho*vrel(2)*vrel(2)*BCYinv; % predicted acceleration measured along Y body axis
    
    % Use a simple viscous drag model for the linear estimator equations
    % Use the the derivative from speed to acceleration averaged across the 
    % speed range对于线性估计方程使用简单的粘性阻力模型
    %使用在速度范围内平均的从速度到加速度的导数
    % The nonlinear equation will be used to calculate the predicted
    % measurement in implementation
    accXpred = -Kaccx*vrel(1); % predicted acceleration measured along X body axis
    accYpred = -Kaccy*vrel(2); % predicted acceleration measured along Y body axis
    
    % Derive observation Jacobian and Kalman gain matrix for X accel fusion
    H_ACCX = jacobian(accXpred,stateVector); % measurement Jacobian
    H_ACCX = simplify(H_ACCX);
    [H_ACCX,SH_ACCX]=OptimiseAlgebra(H_ACCX,'SH_ACCX'); % optimise processing
    K_ACCX = (P*transpose(H_ACCX))/(H_ACCX*P*transpose(H_ACCX) + R_ACC);
    [K_ACCX,SK_ACCX]=OptimiseAlgebra(K_ACCX,'SK_ACCX'); % Kalman gain vector
    
    % Derive observation Jacobian and Kalman gain matrix for Y accel fusion
    H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian
    H_ACCY = simplify(H_ACCY);
    [H_ACCY,SH_ACCY]=OptimiseAlgebra(H_ACCY,'SH_ACCY'); % optimise processing
    K_ACCY = (P*transpose(H_ACCY))/(H_ACCY*P*transpose(H_ACCY) + R_ACC);
    [K_ACCY,SK_ACCY]=OptimiseAlgebra(K_ACCY,'SK_ACCY'); % Kalman gain vector
    
    % save equations and reset workspace
    save('Drag.mat','SH_ACCX','H_ACCX','SK_ACCX','K_ACCX','SH_ACCY','H_ACCY','SK_ACCY','K_ACCY');
    clear all;
    reset(symengine);
    
    %% Save output and convert to m and c code fragments保存输出并转换成m和c代码片段
    
    % load equations for predictions and updates
    load('StateAndCovariancePrediction.mat');
    load('Airspeed.mat');
    load('Sideslip.mat');
    load('Magnetometer.mat');
    load('Drag.mat');
    
    fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
    save(fileName);
    SaveScriptCode(nStates);
    ConvertToM(nStates);
    ConvertToC(nStates);
    © 2018 GitHub, Inc.
    展开全文
  • EKF 卡尔曼滤波matlab源程序 自己跑过,运行没问题
  • EKF/UKF matlab 工具箱

    2009-06-24 09:08:48
    EKF/UKF matlab 工具箱 配合RBMCDA工具箱使用,也可单独使用。
  • matlab模拟EKF滤波器

    2015-04-28 22:40:18
    该程序模拟了三维状态非线性系统的EKF算法。
  • 扩展卡尔曼滤波——非线性EKF-Matlab

    千次阅读 2020-02-11 00:20:27
    扩展卡尔曼滤波EKF仿真实例 这里以无人驾驶的测量障碍物的实际案例为例子展开,如下图所示,毫米波雷达能够测量障碍物在极坐标下离雷达的距离 、方向角以及距离的变化率(径向速度) ,如下图所示, 预测更新 ...

    扩展卡尔曼滤波EKF仿真实例
    这里以无人驾驶的测量障碍物的实际案例为例子展开,如下图所示,毫米波雷达能够测量障碍物在极坐标下离雷达的距离 、方向角以及距离的变化率(径向速度) ,如下图所示,
    在这里插入图片描述
    预测更新
    以二维匀速运动为例,这里的状态量为:
    在这里插入图片描述
    根据牛顿运动定理,经过 [公式] 时间后的预测状态量为:
    在这里插入图片描述
    对于二维的匀速运动模型,加速度为10,并会对预测后的状态造成影响.。
    由此可得状态转移矩阵为
    在这里插入图片描述
    毫米波雷达测量障碍物在径向上的位置和速度相对准确,不确定度较低,因此可以对状态协方差矩阵P进行如下初始化:
    在这里插入图片描述
    由于Q对整个系统存在影响,但又不能太确定对系统的影响有多大。对于简单的模型来说,这里可以直接使用简单矩阵进行计算。

    量测更新
    由图1可知观测值z的数据维度为3×1,为了能够实现矩阵运算,y和Hx’的数据维度也都为3×1。

    使用基本的数学运算可以完成预测值x’从笛卡尔坐标系到极坐标系的坐标转换,求得Hx’,再与观测值z进行计算。需要注意的是,在计算差值y的这一步中,我们通过坐标转换的方式避开了未知的旋转矩阵H的计算,直接得到了Hx’的值。

    为了简化表达,我们用px,py以及vx和vy表示预测后的位置及速度,如下所示:
    在这里插入图片描述
    毫米波雷达观测z是包含位置、角度和径向速度的3x1的列向量,状态向量x’是包含位置和速度信息的4x1的列向量,根据公式y=z-Hx’可知测量矩阵(Measurement Matrix)H的维度是3行4列。即:

    [此时,我们需要对H矩阵进行泰勒展开(线性化),得到其Jacobian矩阵,
    在这里插入图片描述
    经过线性化,最终得到测量矩阵H为:
    在这里插入图片描述

    标准卡尔曼滤波仿真实例请看上篇 :
    (卡尔曼滤波——线性定常) https://editor.csdn.net/md/?articleId=104242921

    下面是Matlab代码实现。
    注:本例所用传感器有激光雷达传感器,雷达传感器

    %EKF无味卡尔曼滤波在自动驾驶车辆定位中的应用
    %参考文献:
    %        The High-Degree Cubature Kalman Filter (Bin Jia, Ming Xin, and Yang Cheng)
    %        Unscented Kalman Filter Tutorial (Gabriel A. Terejanu )
    %        基于正交变换的五阶容积卡尔曼滤波导航算法 (何康辉,董朝阳 )
    %        EKF、UKF和CKF的滤波性能对比研究 (常宇健,赵辰)
    %        组合导航原理与应用 (西北工业大学出版社)
    %        卡尔曼滤波与Matlab仿真 (北京航空航天大学出版社)
    % 本例子使用线性状态模型,因此加速度变成了干扰项
    % Lidar传感器测量模型为线性模型,Radar传感器测模型为非线性模型
    % 在Radar传感器中使用滤波算然为 EKF,在Lidar传感器中使用滤波算然为 KF
    clear;
    clc;
    tic;      %计时
    dt = 0.1; %步长
    
    %分配空间
    Radar_measurement = [];
    Radar_measurement_p = [];
    Lidar_measurement = [];
    EKF_estimation = [];
    ekf = [];  % 结构体
    
    %读取Radar_Lidar_Data2.csv文件保存到矩阵Data
    Data = csvread('Radar_Lidar_Data1.csv',1,1);%起点为第2行第2612组数据
    %将Data矩阵数据保存到data.csv文件中
    csvwrite('Data.csv',Data);
    
    %初始化状态,x是4维向量
    if (Data(1,1) == 1)  
        x = [Data(1,2); Data(1,3); 0; 0];          %如果是lidar测量数据,文件 Dada2.csv,保存这个
    else
        x = [   
                Data(1,2)*cos(Data(1,3)); 
                Data(1,2)*sin(Data(1,3)); 
                Data(1,4)*cos(Data(1,3)); 
                Data(1,4)*sin(Data(1,3))
             ];  %如果是Radar测量数据,文件 Dada1.csv,保存这个
    end
    
    %初始化状态协方差矩阵
    P = diag([1,1,1000,1000]);
     
    %状态转移矩阵,线性
    F = [[1, 0, dt, 0];
         [0, 1, 0, dt];
         [0, 0, 1, 0];
         [0, 0, 0, 1]];
    
     %系统误差协方差矩阵
    Q = [[(dt^2)/4,0,(dt^3)/2,0];
         [0,(dt^2)/4,0,(dt^3)/2];
         [(dt^3/2),0,(dt^2),0];
         [0,(dt^3)/2,0,(dt^2)]];
     
    %Lidar传感器观测矩阵,线性
    H_L = [[1, 0, 0, 0];
           [0, 1, 0, 0]];
    
    %雷达传感器测量误差协方差矩阵
    R_r = diag([0.09,0.005,0.09]);
    
    %激光雷达传感器测量误差协方差矩阵
    R_L = diag([0.0025, 0.0025]);
    
    %4阶单位矩阵
    I = eye(4);
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%开始迭代%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    for n = 1:length(Data)
        
        %Radar传感器
        if (Data(n,1) == 2)
            
            %Radar测量数据用于作图比较
            z = transpose(Data(n,2:4));
            z_p = [ z(1) * cos(z(2));z(1) * sin(z(2));z(3) * cos(z(2));z(3) * sin(z(2))];
            
            %1.预测更新状态、状态协方差阵
            x = F * x;
            P = F * P * transpose(F) + Q;
            
            %2.计算雅可比矩阵
            c1 = x(1)^2 + x(2)^2;
            c2 = sqrt(c1);
            c3 = c1 * c2;
            %测量方程非线性映射
            h = @(x)[c2;
                     atan(x(2)/x(1));
                     (x(1)*x(3)+x(2)*x(4))/c2];
            %避免除0情况
            if (c1==0 || c2==0 || c3==0)
                H_Jac = [[0, 0, 0, 0];
                         [0, 0, 0, 0];
                         [0, 0, 0, 0]];
            else  
                H_Jac = [[x(1)/c2, x(2)/c2, 0, 0];
                        [-x(2)/c1, x(1)/c1, 0, 0];
                        [(x(2)*(x(3)*x(2)-x(4)*x(1)))/c3, (x(1)*(x(1)*x(4)-x(2)*x(3)))/c3, x(1)/c2, x(2)/c2]];
            end
            
            %3.测量更新
            y = z - h(x);  
            %更新卡尔曼增益矩阵
            S = H_Jac * P * transpose(H_Jac) + R_r;
            K = P * transpose(H_Jac) / S;
            %测量更新状态
            x = x + (K * y);
            %更新状态协方差阵
            P = (I - (K * H_Jac)) * P;
            
            %5.保存状态值
            EKF_estimation = [EKF_estimation;transpose(x)];
            Radar_measurement = [Radar_measurement; transpose(z)];
            Radar_measurement_p = [Radar_measurement_p;transpose(z_p)];
        
        %Lidar传感器
        %线性
        else  %(Data(n,1) == 1)情况
            
            %1.预测更新状态、状态协方差阵
            x = F * x;
            P = F * P * transpose(F) + Q;
    
            %2.Radar测量数据
            z = transpose(Data(n,2:3));
            
            %3.测量更新
            %更新卡尔曼增益矩阵
            y = z - (H_L * x);
            S = H_L * P * transpose(H_L) + R_L;
            K = P * transpose(H_L) / S;
            %测量更新状态
            x = x + (K * y);
            %更新状态协方差阵
            P = (I - (K * H_L)) * P;
            
            %4.保存状态值
            EKF_estimation = [EKF_estimation;transpose(x)];
            Lidar_measurement = [Lidar_measurement; transpose(z)];
        end
        
    end
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 数据可视化%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    figure(1);
    hold on;
    plot(Data(:,5),Data(:,6),'k');                                                %真实状态
    scatter(EKF_estimation(:,1),EKF_estimation(:,2),5,'filled','r');                          %估计状态
    scatter(Lidar_measurement(:,1),Lidar_measurement(:,2),5,'filled','blue');     %Lidar传感器测量
    scatter(Radar_measurement_p(:,1),Radar_measurement_p(:,2),5,'filled','g');    %Radar传感器测量
    legend('truth','EKF Path result','Lidar Measurement','Radar Measurement');    %标注
    title('位置估计');
    axis square;
    grid on;
    hold off;
    
    %位置x误差
    figure(2);
    hold on;
    EKF_x_error = Data(1:2:1223,5) - EKF_estimation(1:2:1223,1);  %位置x误差
    plot(1:612,EKF_x_error);
    plot(1:612,Data(1:2:1223,5) - Radar_measurement_p(:,1));
    legend('EKFerror','Radarerror');       
    title('位置x误差');
    grid on;
    hold off;
    
    %位置y误差
    figure(3);
    hold on;
    EKF_y_error = Data(1:2:1223,6) - EKF_estimation(1:2:1223,2);  %位置y误差
    plot(1:612,EKF_y_error);
    plot(1:612,Data(1:2:1223,6) - Radar_measurement_p(:,2));
    legend('EKFerror','Radarerror');       
    title('位置y误差');
    grid on
    hold off;
    
    %速度x误差
    figure(4);
    hold on;
    EKF_vx_error = Data(1:2:1223,7) - EKF_estimation(1:2:1223,3);  %速度x误差
    plot(1:612,EKF_vx_error);
    plot(1:612,Data(1:2:1223,7) - Radar_measurement_p(:,3));
    legend('EKFerror','Radarerror');       
    title('速度x误差');
    grid on
    hold off;
    
    %速度y误差
    figure(5);
    hold on;
    EKF_vy_error = Data(1:2:1223,8) - EKF_estimation(1:2:1223,4);  %速度y误差
    plot(1:612,EKF_vy_error);
    plot(1:612,Data(1:2:1223,8) - Radar_measurement_p(:,4));
    legend('EKFerror','Radarerror');       
    title('速度y误差');
    grid on
    hold off;
    
    %将数据保存到 EKF_error.csv 文件中,用于软法比较
    EKF_error = [EKF_x_error,EKF_y_error,EKF_vx_error,EKF_vy_error];
    csvwrite('EKF_error.csv',EKF_error);
    %保存到结构体
    ekf.estimation = EKF_estimation;
    ekf.error      = EKF_error;
    ekf.measurement_R = Radar_measurement_p;
    ekf.measurement_L = Lidar_measurement;
    %结束计时
    toc;
    
    
    
    

    输出结果:
    在这里插入图片描述
    在这里插入图片描述

    展开全文
  • 主循环函数RunFilter(),输入param,imu_data,mag_data,baro_data,gps_data 一、初始化设置 InitStates(),输入param,imu_data,mag_data,baro_data,gps_data 此函数为导航初始对准时所必需进行的步骤,输出为...
  • 融合部分 GPS融合 考虑GPS延迟情况,选取离IMU当前采样时刻距离最近的GPS的索引latest_gps_index 判断GPS是否blocked ...初次使用GPS信息,判断速度方差是否小于gpsSpdErrLim(设为1),位置方差是否小于...
  • 计算一阶泰勒展开的状态转移矩阵和过程噪声阵,即EKF算法。 计算一步预测协方差矩阵,并在对角线上加过程噪声 。 P = F*P*transpose(F) + Q; P(i,i) = P(i,i) + processNoiseVariance(i); 将P强制对称性 P...
  • ekf ukf tollbox matlab&help

    2010-05-07 10:20:25
    ekf/ukf工具箱及相应的帮助文档ekf/ukf工具箱及相应的帮助文档ekf/ukf工具箱及相应的帮助文档
  • EKF SLAM 例程(matlab

    热门讨论 2010-01-17 15:54:45
    EKF 实现的机器人SLAM 算法,在matlab下仿真,国外牛人的matlab原代码
  • EKF-SLAM-MATLAB代码

    2019-05-06 11:28:53
    MATLAB中使用扩展卡尔曼算法,实现SLAM,可以显示运动轨迹和误差。
  • This is a vrey useful book,which teachs you how to use the EKF in matlab.
  • MATLAB-EKF2

    2018-02-12 13:56:36
    EKF-SLAM基础 问题定义: 小车上搭载一个激光雷达,可以测量出特征点与小车之间的相对位置。利用小车的动力学模型和激光雷达测量数据,估计小车的实际位置和特征点的位置 状态方程列写: 其中动力学模型...
  • EKF机器人定位-MATLAB.m

    2020-01-22 16:04:51
    使用EKF算法(拓展卡尔曼滤波)来估计机器人的位置信息,并实现可视化展示。该EKF算法还与里程计模型和GPS模型估计进行对比,来判断其估计效果。(运行时记得把.m文件改成英文,否则无法运行)
  • ekf-slam matlab环境下

    热门讨论 2010-05-10 20:57:53
    2006年slam会议andrew davaison的演示代码,基于扩展的卡尔曼滤波
  • EKF-SLAM matlab仿真(2)

    万次阅读 多人点赞 2015-10-16 16:38:15
    通过EKF估计机器人的运动
  • EKF.UKF.滤波函数 MATLAB

    2014-01-03 14:16:46
    很全的 MATLAB EKF UKF 代码
  • EKF-SLAM simulated in MATLAB

    2020-07-31 08:15:04
    EKF SLAM中的我们维持的状态量X是由机器人位姿R和路标点M组合而成。 用高斯系统表示的话,就是维护状态量的均值和方差。 均值 协方差 状态量初始化 系统伊始只有机器人状态没有观测,状态量x只包含机器人...

空空如也

空空如也

1 2 3 4 5 ... 11
收藏数 209
精华内容 83
热门标签
关键字:

ekfmatlab

matlab 订阅