资源这里下 Matlab卡尔曼滤波-机器学习文档类资源-CSDN下载
讲到这里还是得感谢苏博,一开始我是稀里糊涂的,跟着他学了会,有一点明白了。
为啥要讲这个呢?别人做了个底盘,结果odom反馈老有问题,没办法了,滤个波吧。
讲一下前提: matlab的版本是linux 2015b的,其他的话会搞 bag2mat(这个怎么用的我也写了个教程)或者你有本事把要看的数据丢表格里都行。
代码一会丢出去,我写的不好,不怎么专业。
插个嘴,整篇都是用Matlab写的,看完的话自己移植到其他平台上也蛮简单的。
基础: 1. 得会把表格或者.mat文件导入到Matlab里,点开Matlab里的导入数据。
正文开始
代码反正都传,免费下,所以我就直接贴代码了。
干这个活儿的目的: 处理不准确的odom角速度
1)认为角速度就只和角速度有关,输入输入就一维
2)认为角速度和线速度也优点关系,输入输出三维
不管一维三维,反正得读数据,先看读数据的部分:
下面写的比较水,随意看吧
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); 这里不讲巴特沃斯滤波器丢了
% 这里直接画出读到的odom 和 imu的反馈
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');
接着将一下 一个参数的卡尔曼滤波和三个参数的卡尔曼滤波的实现
一、 一个参数的卡尔曼滤波的实现
function [Z1] = SINGLEKF(X0, Z0, COV0)
%KF 此处显示有关此函数的摘要
% 此处显示详细说明
Z1 = zeros(size(X0));
P=eye(1); %状态协方差矩阵
F=eye(1); %状态转移矩阵
Q=zeros(1); %状态转移协方差矩矩阵
H=eye(1); %观测矩阵
for i=1:length(Z0)
X_ = F*X0(i);
P_ = P+Q;
K = P_/(P_+COV0);
Z1(i) = X_+K*(Z0(i)-X_);
P = (eye(1)-K*H)*P_;
end
看一下这里其实不涉及矩阵的运算,是不是自己做个滑动窗口去记录 待优化数据、观测数据和协方差 然后把这个function 封一下就能给 C 平台 或者说那些对矩阵运算不友好的平台用了。
二、多输出的卡尔曼滤波的实现
function [Z1] = KF(X0, Z0, COV0)
%KF 此处显示有关此函数的摘要
% 此处显示详细说明
Z1 = zeros(size(X0));
P=eye(3); %状态协方差矩阵
F=eye(3); %状态转移矩阵
Q=zeros(3); %状态转移协方差矩阵
H=eye(3); %观测矩阵
for i=1:length(Z0)
X_ = F*X0(:,i);
P_ = F*P*F'+Q;
K = P_*H'/(H*P_*H'+COV0);
Z1(:,i) = X_+K*(Z0(:,i)-H*X_);
P = (eye(3)-K*H)*P_;
end
看了上面再看这个是不是就很简单了,只是把单个数据换成向量了。至于移植到其他平台,能支持矩阵运算的话移植也很简单,和上面那个差不多了