简单的卡尔曼滤波器

资源这里下 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

看了上面再看这个是不是就很简单了，只是把单个数据换成向量了。至于移植到其他平台，能支持矩阵运算的话移植也很简单，和上面那个差不多了