精华内容
下载资源
问答
  • KF+EKF matlab程序实现

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

    2019-01-09 15:36:49
    PX4 EKF MATLAB代码参考,建立以角速度,角加速度,重力加速度,磁通量共12阶的数据为状态的状态方程。以角速度、加速度、磁通量建立量测方程进行EKF滤波
  • 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
      
    % 扩展Kalman滤波在目标跟踪中的应用
      
    % functionEKF_For_TargetTracking
      
    clc ; clear ;
      
    T = 1 ; % 雷达扫描周期
      
    N = 60 / T ; % 总的采样次数
      
    X = zeros ( 4 , N ) ; % 目标真实位置、速度
      
    X ( : , 1 ) = [ - 100 , 2 , 200 , 20 ] ; % 目标初始位置、速度
      
    Z = zeros ( 1 , N ) ; % 传感器对位置的观测
      
    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实现

    展开全文
  • Matlab集成的c代码IMU_EKF 该代码通过Matlab代码复制了论文“用于IMU.pdf的双级卡尔曼滤波器”或“用于9D IMU集成处理器的定向跟踪的双级卡尔曼滤波器”中介绍的工作。 然后使用Matlab C Coder将matlab代码转换为C...
  • EKF MATLAB程序

    热门讨论 2011-04-17 12:31:32
    简单的扩展卡尔曼滤波程序,适合于刚刚接触 MATLAB编程的同学参考一下
  • 数据融合matlab代码扩展卡尔曼滤波器,用于物体跟踪 无人驾驶汽车工程师纳米学位课程 在这个项目中,我使用了扩展卡尔曼滤波器,通过带噪的激光雷达和雷达测量来估计感兴趣的运动物体的状态。 使用文件“ obj_pose-...
  • EKF MATLAB函数

    2015-04-13 16:13:23
    不要写EKF代码,只要输入系统方程和量测方程即可滤波
  • UKF KF EKF MATLAB仿真

    2012-10-18 16:39:11
    介绍了卡尔曼滤波及各种改进算法,包括EKF,UKF等,并给出matlab仿真
  • 备注: 扩展卡尔曼滤波在目标跟踪中的应用 ...MATLAB仿真 目标跟踪matlab仿真实现; Case: 二维目标跟踪情况 对应的理论分析和参数设置,见博文https://blog.csdn.net/weixin_44044161/article/details/115313573
  • 扩展卡尔曼滤波(EKF)仿真中实现对雷达目标的跟踪matlab.zip
  • EKF 卡尔曼滤波matlab源程序 自己跑过,运行没问题
  • ekf matlab仿真

    2012-06-28 09:45:57
    ekf matlab 关于efk仿真,matlab源代码
  • EKF,UKF的matlab实现

    2021-04-23 15:11:17
    【实例简介】无迹卡尔曼滤波和扩展卡尔曼滤波的matlab仿真程序,验证后可用【实例截图】【核心代码】ekfukf└── ekfukf├── Contents.m├── License.txt├── Release_Notes.txt├── Release_Notes.txt~├...

    【实例简介】

    无迹卡尔曼滤波和扩展卡尔曼滤波的matlab仿真程序,验证后可用

    【实例截图】

    【核心代码】

    ekfukf

    └── ekfukf

    ├── Contents.m

    ├── License.txt

    ├── Release_Notes.txt

    ├── Release_Notes.txt~

    ├── cancer

    │   ├── cancer_test.m

    │   └── cancer_test.m~

    ├── demos

    │   ├── bot_demo

    │   │   ├── bot_d2h_dx2.m

    │   │   ├── bot_demo_all.m

    │   │   ├── bot_dh_dx.m

    │   │   ├── bot_h.m

    │   │   ├── ekfs_bot_demo.m

    │   │   └── ukfs_bot_demo.m

    │   ├── eimm_demo

    │   │   ├── bot_d2h_dx2.m

    │   │   ├── bot_dh_dx.m

    │   │   ├── bot_h.m

    │   │   ├── botm_demo.m

    │   │   ├── ct_demo.m

    │   │   ├── f_turn.m

    │   │   ├── f_turn_dx.m

    │   │   ├── f_turn_inv.m

    │   │   └── trajectory.mat

    │   ├── ekf_sine_demo

    │   │   ├── ekf_sine_d2h_dx2.m

    │   │   ├── ekf_sine_demo.m

    │   │   ├── ekf_sine_dh_dx.m

    │   │   ├── ekf_sine_f.m

    │   │   └── ekf_sine_h.m

    │   ├── imm_demo

    │   │   ├── imm_demo.m

    │   │   └── trajectory.mat

    │   ├── kf_cwpa_demo

    │   │   └── kf_cwpa_demo.m

    │   ├── kf_sine_demo

    │   │   └── kf_sine_demo.m

    │   ├── reentry_demo

    │   │   ├── make_reentry_data.m

    │   │   ├── reentry_cond.m

    │   │   ├── reentry_demo.m

    │   │   ├── reentry_demo.m~

    │   │   ├── reentry_df_dx.m

    │   │   ├── reentry_dh_dx.m

    │   │   ├── reentry_f.m

    │   │   ├── reentry_h.m

    │   │   ├── reentry_if.m

    │   │   └── reentry_param.m

    │   └── ungm_demo

    │   ├── ungm_d2f_dx2.m

    │   ├── ungm_d2h_dx2.m

    │   ├── ungm_demo.m

    │   ├── ungm_df_dx.m

    │   ├── ungm_dh_dx.m

    │   ├── ungm_f.m

    │   └── ungm_h.m

    ├── der_check.m

    ├── eimm_filter.m

    ├── eimm_predict.m

    ├── eimm_smooth.m

    ├── eimm_update.m

    ├── ekf_predict1.m

    ├── ekf_predict2.m

    ├── ekf_update1.m

    ├── ekf_update2.m

    ├── erts_smooth1.m

    ├── etf_smooth1.m

    ├── gauss_pdf.m

    ├── gauss_rnd.m

    ├── imm_filter.m

    ├── imm_predict.m

    ├── imm_smooth.m

    ├── imm_update.m

    ├── immrts_smooth.m

    ├── kf_lhood.m

    ├── kf_loop.m

    ├── kf_predict.m

    ├── kf_update.m

    ├── lti_disc.m

    ├── lti_int.m

    ├── resampstr.m

    ├── rk4.m

    ├── rts_smooth.m

    ├── schol.m

    ├── tf_smooth.m

    ├── uimm_predict.m

    ├── uimm_smooth.m

    ├── uimm_update.m

    ├── ukf_predict1.m

    ├── ukf_predict2.m

    ├── ukf_predict3.m

    ├── ukf_update1.m

    ├── ukf_update2.m

    ├── ukf_update3.m

    ├── urts_smooth1.m

    ├── urts_smooth2.m

    ├── ut_mweights.m

    ├── ut_sigmas.m

    ├── ut_transform.m

    ├── ut_weights.m

    └── utf_smooth1.m

    11 directories, 92 files

    展开全文
  • 为了评估所提出的基于三基站观测距离EKF融合滤波算法和传统的三基站观测距离LS定位算法的性能,假设系统的过程噪声和观测噪声都是服从均值为零,方差分别为10-8m和10-2m且彼此独立分布的高斯白噪声。已知三个参考...
  • 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工具

    2009-08-13 19:14:26
    扩展卡尔曼滤波简介, EKFmatlab例程
  • EKF:主要EKF功能 sampledata:我使用的sampledata 如何使用 1.从IMU传感器获取数据。 您应该在开始的5秒钟内保持IMU传感器处于静止状态(用于陀螺仪补偿),然后旋转它。 我建议您将其旋转几次。 (这对磁力计...
  • 聪明的EKF算法matlab代码一种高效有效的二阶训练算法,用于基于 LSTM 的自适应学习 该软件随附论文 Vural, NM, Ergut, S., Kozat SS (2020),“基于 LSTM 的自适应学习的高效和有效的二阶训练算法”,IEEE 信号处理...
  • 最受欢迎的见解 1.matlab使用经验模式分解emd 对信号进行去噪 2.Matlab使用Hampel滤波去除异常值 3.matlab偏最小二乘回归(PLSR)和主成分回归(PCR) 4.matlab预测ARMA-GARCH 条件均值和方差模型 5.matlab中使用VMD...

    原文链接:http://tecdat.cn/?p=22467 

    原文出处:拓端数据公众号

    本文展示了如何使用扩展卡尔曼滤波器进行故障检测。本文使用扩展的卡尔曼滤波器对一个简单的直流电机的摩擦力进行在线估计。估计的摩擦力的重大变化被检测出来,并表明存在故障。

    电机模型

    电机被模拟成具有阻尼系数c,转动惯量J,由一个扭矩u驱动。电机的角速度w和加速度˙w是测量输出。

    为了使用扩展卡尔曼滤波器估计阻尼系数c,为阻尼系数引入一个辅助状态,并将其导数设为零。

    因此,模型状态,x = [w;c],和测量,y,方程式为:

    使用近似值˙x=xn+1-xnTs将连续时间方程转换为离散时间,其中Ts为离散采样周期。这就得到了离散时间模型方程。

    指定电机参数

    J  = 10;    % 惯性
    Ts = 0.01;  % 采样时间

    指定初始状态。

    x = [...
     0; ... % 角速度 
        1]; % 摩擦力
    
    
    % 以摩擦为状态的电机的状态更新方程
    % 状态更新方程式
    x1 = [...
        x0(1)+Ts/J*(u-x0(1)*x0(2)); ...
        x0(2)];
     
    
    % 以摩擦为状态的电机的测量方程
    
    % 输出。
    % y - 电机测量元素[角速度;角加速度]。
    
    y = [...
        x(1); ...
        (u-x(1)*x(2))/J];
    

    电机经历状态(过程)噪声干扰,q,和测量噪声干扰,r。噪声项是相加的。

    过程和测量噪声的平均值为零,E[q]=E[r]=0,协方差Q=E[qq']和R=E[rr']。摩擦状态有很高的过程噪声干扰。这反映了我们希望摩擦力在电机正常运行期间会有变化,并且滤波器能跟踪这种变化。加速和速度状态的噪声很低,但速度和加速度测量的噪声相对较大。

    指定过程噪声协方差。

    [...
        1e-6 0; ...   % 角速度
    
        0 1e-2];      % 摩擦力

    指定测量噪声协方差。

    [...
        1e-4 0; ...  % 速度测量
        0 1e-4];     % 加速度测量

    创建一个扩展的卡尔曼过滤器

    创建一个扩展的卡尔曼滤波器来估计模型的状态。我们特别关注阻尼状态,因为这个状态值的急剧变化表明存在故障事件。
    创建一个扩展卡尔曼滤波器对象,并指定状态转换和测量函数的雅各布系数。

    扩展卡尔曼滤波器的输入参数是之前定义的状态转换和测量函数。初始状态值x0、初始状态协方差、过程和测量噪声协方差也是扩展卡尔曼滤波器的输入。在这个例子中,精确的雅各布函数可以从状态转换函数f和测量函数h中得到。

    
    % 输出
    Jac - 在x处计算出的状态雅各布系数
    
    
    % 雅各布系数
    Jac = [
        1-Ts/J*x(2) -Ts/J*x(1); ...
        0 1];
    
    % 电机模型测量方程的雅各布系数
    
    % 输出
    Jac - 在 x 处计算的测量雅各布系数
    
    
    % 雅各布系数
    J = [ ...
        1 0;
        -x(2)/J -x(1)/J];
    

    Simulation仿真

    为了模拟工厂,创建一个环路,在电机中引入一个故障(虚构的电机剧烈变化)。在模拟回路中,使用扩展的卡尔曼滤波器来估计电机状态,并特别跟踪摩擦状态,检测摩擦力何时发生统计意义上的变化。

    电机被模拟成一个脉冲序列,反复加速和减速。这种类型的电机操作对于生产线上的采摘机器人来说是典型的。

    在模拟电机时,加入与构建扩展卡尔曼滤波器时使用的Q和R噪声协方差值相似的过程和测量噪声。对于摩擦,使用一个小得多的噪声值,因为除了故障发生时,摩擦大多是恒定的。在模拟过程中人为地诱发故障。

    Qv = chol(Q); % 过程噪声的标准偏差
    Qv(end) = 1e-2; % 较小的摩擦噪声
    Rv = chol(R); % 测量噪声的标准偏差

    使用状态更新方程对模型进行仿真,并在模型状态中加入过程噪声。仿真十秒钟后,强制改变电机的摩擦力。使用模型测量功能来模拟电机传感器,并在模型输出中加入测量噪声。

    for ct = 1:numel(t)  
     % 模型输出更新   
       y = y+Rv*randn(2,1); % 添加测量噪声
       % 模型状态更新 
       xSig(:,ct) = x0;
       % 诱发摩擦力的变化
       if t(ct) == 10
           x1(2) = 10; % 步骤变化
     
       x1n = x1+Qv*randn(nx  % 加入过程噪声
    

    Significant friction change at 10.450000
    

    为了从电机测量值中估计电机状态,使用扩展卡尔曼滤波器的预测和纠正命令。

    
    
      % 使用扩展卡尔曼滤波器进行状态估计
       x_corr = correct(ekf,y,u(ct),J,Ts); % 根据当前测量结果修正状态估计。
       predict(ekf,u(ct),J,Ts); % 根据当前状态和输入预测下一个状态。

    为了检测摩擦力的变化,使用4秒的移动窗口来计算估计的摩擦力平均值和标准偏差。在最初的7秒之后,锁定计算的平均值和标准差。这个最初计算出的平均值是摩擦力的预期无故障平均值。7秒后,如果估计的摩擦力与预期的无故障平均值相差超过3个标准差,这就意味着摩擦力有了明显的变化。为了减少噪音和估计摩擦力的影响,在与3个标准差的界限比较时,使用估计摩擦力的平均值。

    % 计算估计平均值和标准偏差。
       else
           % 存储计算出的平均数和标准差,不需要
           %重新计算。
           fMean(ct) = fMean(ct-1) 
           % 使用预期的摩擦力平均值和标准偏差来检测
           %摩擦力变化。
           estFriction = mean(xSigEst(2, 
       if fChanged(ct) && ~fChanged(ct-1) 
           % 检测摩擦变化信号的上升沿|fChanged|
    

    使用估计的状态来计算估计的输出。计算测量输出和估计输出之间的误差,并计算出误差统计。误差统计可用于检测摩擦力的变化。这一点将在后面详细讨论。

     
           kurtosis(ySigEst(1,idx)-ySig(1,idx));  
           kurtosis(ySigEst(2,idx)-ySig(2,idx))];
     

    扩展的卡尔曼滤波器性能

    请注意,在10.45秒时检测到了一个摩擦变化。我们现在描述一下这个故障检测规则是如何得出的。首先检查仿真结果和过滤器的性能。

    figure, 
     plot(t,  Sig(1,:)  Sig(2,:));
     

    模型的输入输出响应表明,很难直接从测量信号中检测出摩擦力的变化。扩展的卡尔曼滤能够估计状态,特别是摩擦状态。比较真实的模型状态和估计状态。估计的状态显示了对应于3个标准差的置信区间。

    plot(t, True(1,:), t,  Est(1,:), ...
    
     

    请注意,滤波器的估计值跟踪了真实值,而且置信区间仍然有界。检查估计误差可以更深入地了解滤波器。

    plot(t,True(1,:)-Est(1,:)
     

    误差图显示,滤波器在10秒的摩擦力变化后进行了调整,并将估计误差降低到了零。然而,误差图不能用于故障检测,因为它们依赖于对真实状态的了解。将测量的状态值与加速度和速度的估计状态值进行比较,可以提供一种检测机制。

    plot(t,Sig(1,:-Est(1,:)
    

    加速度误差图显示,在引入故障的10秒左右,平均误差有微小的差异。查看误差统计,看看是否可以从计算的误差中检测出故障。加速度和速度误差预计是正态分布的(噪声模型都是高斯的)。因此,加速度误差的峰度可能有助于识别由于摩擦力的变化和由此产生的误差分布从对称变为不对称的变化情况。

    plot(t,Kur(1,:)
    

    忽略估计器仍在收敛和收集数据的前4秒,误差的峰度相对稳定,在3(高斯分布的预期峰度值)附近有微小的变化。因此,在这个应用中,误差统计不能被用来自动检测摩擦力的变化。在这个应用中,使用误差的峰度也是很困难的,因为过滤器正在适应并不断地将误差推向零,只给出了一个误差分布与零不同的短暂时间窗口。

    因此在这个应用中,使用估计的摩擦力的变化提供了自动检测电机故障的最好方法。来自已知无故障数据的摩擦力估计值(平均值和标准偏差)提供了摩擦力的预期界限,当这些界限被超过时,很容易检测出来。下面的图强调了这种故障检测方法。

    plot(t,x,[nan t],[Mean+3*STD,Mean-3*STD]
    

    摘要

    这个例子展示了如何使用扩展的卡尔曼滤波器来估计一个简单的直流电动机的摩擦力,并使用摩擦力估计值进行故障检测。


    最受欢迎的见解

    1.matlab使用经验模式分解emd 对信号进行去噪

    2.Matlab使用Hampel滤波去除异常值

    3.matlab偏最小二乘回归(PLSR)和主成分回归(PCR)

    4.matlab预测ARMA-GARCH 条件均值和方差模型

    5.matlab中使用VMD(变分模态分解) 

    6.matlab使用贝叶斯优化的深度学习

    7.matlab贝叶斯隐马尔可夫hmm模型

    8.matlab中的隐马尔可夫模型(HMM)实现

    9.matlab实现MCMC的马尔可夫切换ARMA – GARCH模型

    展开全文
  • 【实例简介】【实例截图】【核心代码】EKF_AHRS-master├── EKF-AHRS│ ├── EKF_AHRS.m│ ├── Get_Init_AHRS.m│ ├── Get_Yaw.m│ ├── datasets│ │ └── ADIS16465-rm3100-2020-01-11-18-57-yaw90...
  • 完整的扩展卡尔曼MATLAB源程序,非常适合初学卡尔曼的学生使用!
  • 九轴传感器姿态解算方法(MATLAB
  • EKF机器人定位-MATLAB.m

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

    万次阅读 多人点赞 2017-05-04 15:35:01
    % Kalman filter example of temperature measurement in Matlab implementation of Kalman filter algorithm. % 房间当前温度真实值为24度,认为下一时刻与当前时刻温度相同,误差为0.02度(即认为连续的两个时刻...
  • EKF_matlab

    2013-10-15 13:21:28
    具体说明请见 http://blog.csdn.net/apsvvfb/article/details/12651999
  • 扩展卡尔曼滤波(EKF)理论讲解与实例(matlab、python和C++代码) 文章目录扩展卡尔曼滤波(EKF)理论讲解与实例(matlab、python和C++代码)理论讲解KF和EKF模型对比雅可比矩阵计算计算实例应用实例线性模型CV模型...
  • (一)扩展卡尔曼滤波(EKF)算法及简单仿真(matlab
  • 无迹卡尔曼滤波(UKF), 容积卡尔曼滤波(CKF), 扩展卡尔曼滤波(EKF)三种卡尔曼滤波算法的比较,使用的是matlab代码
  • 备注: 扩展卡尔曼滤波在三维目标跟踪中的应用 ...MATLAB仿真 目标跟踪matlab仿真实现; Case: 二维目标跟踪情况 对应的理论分析和参数设置,见博文https://blog.csdn.net/weixin_44044161/article/details/115313573

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 1,141
精华内容 456
关键字:

ekfmatlab

matlab 订阅
友情链接: DCT.rar