• 马小靓
    了解作者
  • matlab
    开发工具
  • 1KB
    文件大小
  • zip
    文件格式
  • 0
    收藏次数
  • 1 积分
    下载积分
  • 2
    下载次数
  • 2020-09-18 15:24
    上传日期
基于卡尔曼滤波算法,估计直线行驶小车的状态,即位置和速度。
kalman.zip
  • kalman.m
    2.4KB
内容介绍
function kalman(duration,dt) % function kalman(duration,dt) % kalman filter simulation for a vehicle travelling along a road. % inputs % duration=length of simulation(seconds) % dt=step size(seconds) duration=60; dt=0.01; measnoise=10; % position measurement noise(feet) accelnoise=0.2;% acceleration noise (feet/sec^2) a=[1 dt;0 1];% transition matrix b=[dt^2/2;dt];% input matrix c=[1 0];% measurement matrix x=[0;0];% initial state vector xhat=x;% initial state estimate Sz=measnoise^2;% measurement error covariance Sw=accelnoise^2*[dt^4/4 dt^3/2;dt^3/2 dt^2];% process noise cov P=Sw;% initial estimation covariance % initialize array for later plotting. pos=[];% true position array poshat=[];% estimated position array posmeas=[];% measured position array vel=[];% true velocity array velhat=[];% estimated velocity array for t=0:dt:duration % use a constant command acceleration of 1 feet/sec^2. u=1; % simulate the linear system. ProcessNoise=accelnoise*[(dt^2/2)*randn;dt*randn]; x=a*x+b*u+ProcessNoise; MeasNoise=measnoise*randn; y=c*x+MeasNoise; % extrapolate(推测) the most recent stae estimate to the present time. xhat=a*xhat + b*u; % 预测 % form the innovation vector. Inn=y - c*xhat; % compute the covariance of the innovation. s=c*P*c'+Sz; % form the kalman gain matrix. K=P*c'*inv(s); % update the state estimate. xhat=xhat + K*Inn; % compute the covariance of the estimation error. P=(eye(2)-K*c)*P; % save some parameters for plotting later. pos=[pos;x(1)]; posmeas=[posmeas;y]; poshat=[poshat;xhat(1)]; vel=[vel;x(2)]; velhat=[velhat;xhat(2)]; end % plot the results close all; t=0:dt:duration; figure; plot(t,pos,t,posmeas,t,poshat); grid; xlabel('Time(sec)'); ylabel('Position(feet)'); title('Figure 1-vehicle position(true,measured,and estimated)'); figure; plot(t,pos-posmeas,t,pos-poshat); grid; xlabel('Time(sec)'); ylabel('Position error(feet)'); title('Figure 2-position measurement error and position estimation error'); figure; plot(t,vel,t,velhat); grid; xlabel('Time(sec)'); ylabel('Velocity(feet/sec)'); title('Figure 3-velocity(true and estimated)'); figure; plot(t,vel-velhat); grid; xlabel('Time(sec)'); ylabel('Vehicle error(feet/sec)'); title('Figure 4-velocity estimation error');
评论
    相关推荐
    • cabature kalman.zip
      容积卡尔曼滤波测量方程基于统计数据,利用蒙特卡罗方法抽取私家电动汽车一次出行里程数,根据电池充电特性及车辆行驶习惯获得电动汽车充电的起始荷电状态、充电功率和起始充电时间,建立了一个较为精确的预测无线...
    • kalmanfilter.rar
      卡尔曼滤波算法,英文文章原文,适合初学者。
    • 双边滤波.zip
      双边滤波,包含效果图,以及其matlab实现,多种论文供参考
    • Kalman.rar
      在数据有丢失的情况下,用kalman滤波实现状态估计
    • kalman积分.rar
      通过matlab软件编写的数值积分算法,积分采用kalman滤波算法实现
    • kalman-localization-master.zip
      卡尔曼滤波,给出解释说明,并做出仿真图形,附件中含有文档说明
    • estimate.zip
      EKF算法扩展卡尔曼滤波 对应s函数模块算法及相关论文
    • 卡尔曼滤波的slam
      这是一个在MATLAB上运行的基于卡尔曼滤波的slam的程序,包括2D、3D
    • Kalman_AHRS.rar
      基于STM32F103ZE+mpu6050+hmc5883L通过卡尔曼滤波串口输出姿态角,数据经过融合,Z轴无漂移
    • matlabcnhelp.rar
      matlab中文帮助很难找的,快速下载