强跟踪卡尔曼滤波STF估算车辆质量——matab simulink仿真

最新推荐文章于 2024-07-31 16:49:23 发布

lijil168

最新推荐文章于 2024-07-31 16:49:23 发布

阅读量6.9k

点赞数 9

分类专栏：机械数学 matlab 文章标签：算法概率论卡尔曼滤波强跟踪滤波车辆质量估算

本文链接：https://blog.csdn.net/lijil168/article/details/103832234

版权

数学同时被 3 个专栏收录

120 篇文章 20 订阅

订阅专栏

机械

85 篇文章 15 订阅

订阅专栏

matlab

56 篇文章 20 订阅

订阅专栏

强跟踪卡尔曼滤波估算车辆质量——matab simulink仿真

链接： https://pan.baidu.com/s/1VxxBkJqBI8nfrTJaNi2uKw
提取码：1234

仿真步长为0.1s,卡尔曼滤波估算的车速和质量见下图，3s左右达到稳态，当质量在5s发生突变后，卡尔曼滤波对车速和质量的跟踪比较慢。
在这里插入图片描述
采用强跟踪卡尔曼滤波器估算的车速与质量如下图，当质量在5s发生突变后，强跟踪卡尔曼滤波对车速和质量的跟踪比较快。正确地估算出了结果。

车辆运动状态仿真代码

%kalman_sim.m
%Plant
function [u]=kalman_sim(u1,u2,u3,u4)
%u1:输入Ttq
Ttq=u1;
%u2:系统噪声W
W=u2;
%u3:测量噪声V
V=u3;
%u4:时间t
t=u4;

persistent v m i Ts ig i0 nn g f r CD A rou qq
if t==0
    Ts=0.1;
    ig=1.0;
    i0=6.83;
    nn=0.95*0.96;
    g=9.8;
    f=0.011;
    r=0.502;
    CD=0.8;%%%%%卡车取0.8 客车0.65
    A=1.92*3.09;
    rou=1.2258;
    qq=1.027+0.000331*i0^2.*ig.^2;%%汽车质量转换系数
    
    v=0.0;
    m=60000.0;
    i=0.0;
    z=v+V;
end
if t>5
    m=20000.0;%假设5秒后质量出现突变2020
end
if t>0
    dv=1/qq*(Ttq*ig*i0*nn/(m*r)-g*f-CD*A*rou*v^2/(2*m)-g*i);
    v=v+Ts*dv+Ts*W;
    z=v+V;
end

u(1)=v;
u(2)=m;
u(3)=i;
u(4)=z;

强跟踪卡尔曼滤波代码

%kalman_m_i.m
%Plant
function [u]=kalman_sim(u1,u2,u3)
%u1:输入Ttq
Ttq=u1;
%u2:系统噪声W
z=u2;
%u3:时间t
t=u3;

%persistent v m i
persistent x Ts ig i0 nn g f r CD A rou qq Q R P H Jf V0
if t==0
    Ts=0.1;
    ig=1.0;
    i0=6.83;
    nn=0.95*0.96;
    g=9.8;
    f=0.011;
    r=0.502;
    CD=0.8;%%%%%卡车取0.8 客车0.65
    A=1.92*3.09;
    rou=1.2258;
    qq=1.027+0.000331*i0^2.*ig.^2;%%汽车质量转换系数

    %系统方程：
    
    H=[1.0,0.0,0.0];  
      
    %Covariances of w:
    Q=diag([Ts*0.1*Ts',0,0]);

    %Covariances of v:
    R=[0.1];

    %初始估计值：
    x=[0;30000;0.0];

    %初始估计误差协方差：
    P=diag([Ts*0.1*Ts',9000000000,0.000000]); 
    
    u=x;%开始时刻不进行卡尔曼滤波，直接将初始值输出,2020
end

%%卡尔曼滤波：：
if t>0
    %先验，Time update:
    %根据系统状态方程计算下一状态预测值：
    %x=A*x+B*u1;
    Jf=eye(3);
    Jf(1,1)=1-CD*A*rou*x(1)*Ts/x(2)/qq;%add /qq 2020
    Jf(1,2)=(CD*A*rou*x(1)^2*r-2*Ttq*ig*i0*nn)/(2*x(2)^2*r)*Ts/qq;%add /qq 2020
    Jf(1,3)=-g*Ts; 

    v=x(1);
    m=x(2);
    i=x(3);
    dv=1/qq*(Ttq*ig*i0*nn/(m*r)-g*f-CD*A*rou*v^2/(2*m)-g*i);
    v=v+Ts*dv;
    x(1)=v;
    x(2)=m;
    x(3)=i;
 
    %************强跟踪滤波器STF核心代码《：：*********************************
    %**************************************
    % 可调节参数汇总
    %**************************************
    rho=0.95;                         % 残差方差阵的遗忘因子初始值 (0.95<rho<0.995)
    beta=1;                           % 量测噪声的衰减因子选定值 (beta>=1)
    %beta_upmax=8;                     % 衰减因子选定极限
    %***************************************
    F=Jf;
    r0=z-H*x;
    if t==Ts
        V0=r0*r0';
    else
        V0=(rho*V0+r0*r0')/(1+rho);
    end
    N=V0-beta*R-H*Q*H';
    M=H*F*P*F'*H';
    eta=trace(N)/trace(M);
    if eta>1
        lamda=1;%eta;
    else
        lamda=1;
    end
    
    %预测误差协方差：
    P=lamda*Jf*P*Jf'+Q;  
    %！！！渐消因子lamda=1即为普通卡尔曼滤波！！！
    %************STF核心代码》************************************************
    
    %后验，Measurement update:
    %根据估计误差协方差和测量噪声协方差计算卡尔曼增益：
    Kk=P*H'/(H*P*H'+R);

    %计算最优估计值：    
    %x=A*x+Mn*(yv-C*A*x);
    x=x+Kk*(z-H*x);

    %计算最优估计值和真实值之间的误差协方差矩阵，为下次递推做准备：
    P=(eye(3)-Kk*H)*P;

    %ye=C*x;           %Filtered value
    u=x;    %Filtered signal

    errcov=H*P*H';      %Covariance of estimation error   
    
end
%}