Matlab 卡尔曼滤波

n_start = 5000; %从第几个数据开始取 n_end = 5500; %取到第几个个数据 odom_fd_z = robot.odom_data.angular_velocity_z(n_start:n_end); % odom 直接反馈的 YAW角速度 odom_fd_x = robot.odom_data.velocity_x(n_start:n_end); % odom 直接反馈的 YAW角速度 odom_fd_y = robot.odom_data.velocity_y(n_start:n_end); % odom 直接反馈的 YAW角速度 imu_fd_z = robot.imu_data.angular_velocity_z(n_start:n_end); % imu 直接反馈的 YAW角速度 %odom_fd_z_filtered = BUTFILTER(odom_fd_z); 这里不讲巴特沃斯滤波器丢了 figure(1); x = 1:1:(n_end-n_start+1); plot(x,imu_fd_z,'b'); hold on; plot(x,odom_fd_z,'r'); hold on; % plot(x,odom_fd_z_filtered,'k'); % hold on; legend('blue imu_fd_z red odom_fd_z '); N=n_end-n_start+1; MY_CMD = [0.4*ones(N,1) zeros(N,1) 0.2*ones(N,1)]'; %我录的包线速度0.4m/s 角速度0.2rad/s所以这么写 for i = 1:(n_end-n_start+1) odom_fd_y(i) = odom_fd_y(i)+randn ; %加个满足正态分布的随机误差 end MY_FD = [odom_fd_x,odom_fd_y,odom_fd_z]; % 获取x y z 方向速度反馈 MY_CPV = cov(MY_FD); % 求协方差 MY_FD_T = MY_FD'; RESULT_SINGLE = SINGLEKF(0.2*ones(N,1)', odom_fd_z', cov(odom_fd_z) ); % 做一次一个输入的卡尔曼滤波 figure(2); plot(x,RESULT_SINGLE','g'); hold on; plot(x,imu_fd_z,'b'); hold on; plot(x,odom_fd_z,'r'); legend('blue imu_fd_z red odom_fd_z green single kf filter'); RESULT = KF(MY_CMD, MY_FD_T, cov(MY_FD_T') ); % 做一次三输入的卡尔曼滤波 figure(3); plot(x,RESULT(3,:)','g'); hold on; plot(x,imu_fd_z,'b'); hold on; plot(x,odom_fd_z,'r'); hold on; legend('blue imu_fd_z red odom_fd_z green kf filter');