zerok 发表于 2011-11-23 20:16:07

卡尔曼滤波

% 建模数据；
% 陀螺仪选用ADXRS150，灵敏度为15mv/°/s；
% 采样点个数 N=2500；
% 采样周期 T=0.002s；
% angle_KF 经过滤波后的角度；
% velocity_KF 经过滤波后的角速率；
% t=0.002:0.002:5;    % 共5秒时间；
% T=0.002;            % 采样周期；
% X(k) = AX(k-1) + Bw(k-1);
% X(k) = ;
% Z(k) = HX(k) + v(k);
% X(k) = ;

A = ;
B = ;
H = ;
Q = 0.05;
R = 0.05;
T = 0.002;      %采样周期0.002秒
N = 2500;       %采样点个数：2500
load data2 data2;               % 陀螺仪读数（电压值），1*2500
test_velocity_vol = data2;
velocity_vol = test_velocity_vol - (2.5 * ones(1,N)); %陀螺仪零点电压约为2.65V
velocity(1) = velocity_vol(1) / (15/1000);    % 角速率初值
angle(1) = velocity(1)*T;                   % 角度初值
for n = 2:N;
    velocity(n) = velocity_vol(n) / (15 / 1000);
    angle(n) = angle(n-1) + velocity(n)*T - 0.02;%这里-0.02和不减差距很大，具体见图片
end
status = ;          % 系统状态，即观测值
X = zeros(2,N);
X(:,1) = ;
C0 = ;
C = ;

for n = 1:N;
    m = (n-1)*2+1;
    X(:,n+1) = A*X(:,n);                        %自己写的卡尔曼算法
    C(:,m:m+1) = A*C(:,m:m+1)*A'+B*Q*B';
    Kg = C(:,m:m+1)*H'*inv(H*C(:,m:m+1)*H'+R);
    X(:,n+1) = X(:,n)+Kg*(status(:,n)-H*X(:,n));
    C(:,m+2:m+3) = (eye(2,2)-Kg*H)*C(:,m:m+1);
   % Kg = C(:,m:m+1)*H'*inv(H*C(:,m:m+1)*H'+R);网上程序的卡尔曼算法
   % X(:,n) = X(:,n)+Kg*(status(:,n)-H*X(:,n));   
   % X(:,n+1) = A*X(:,n);
   % C(:,m:m+1) = (eye(2,2)-Kg*H)*C(:,m:m+1);
   % C(:,m+2:m+3) = A*C(:,m:m+1)*A'+B*Q*B';
end
figure(6);
t = 0.002:0.002:5;
angle_KF = X(1,2:2501);
velocity_KF = X(2,2:2501);
plot(t,angle_KF,'r',t,angle,'b',t,velocity_KF,'m',t,velocity,'g');
legend('angle after KF','angle','velocity after KF','velocity');
grid;
xlabel('time');
ylabel('four status');
title('KF VS NoKF');
程序思维：首先获取2500组ADXRS150的数据，然后算出其角速率和对应的角度变化，并作为卡尔曼滤波的观测值：
load data2 data2;               % 陀螺仪读数（电压值），1*2500
test_velocity_vol = data2;
velocity_vol = test_velocity_vol - (2.5 * ones(1,N)); %陀螺仪零点电压约为2.65V
velocity(1) = velocity_vol(1) / (15/1000);    % 角速率初值
angle(1) = velocity(1)*T;                   % 角度初值
for n = 2:N;
    velocity(n) = velocity_vol(n) / (15 / 1000);
    angle(n) = angle(n-1) + velocity(n)*T - 0.02;%这里-0.02和不减差距很大，具体见图片
end
再就是进行滤波，其初值设置如下：
A = ;
B = ;
H = ;
Q = 0.05;
R = 0.05;
T = 0.002;      %采样周期0.002秒
N = 2500;       %采样点个数：2500
图一是减去0.02的滤波效果图：
http://cache.amobbs.com/bbs_upload782111/files_48/ourdev_698381A967UV.jpg
(原文件名:图1.jpg)
图二是没减0.02的滤波效果图：
http://cache.amobbs.com/bbs_upload782111/files_48/ourdev_698382Y7OEEU.jpg
(原文件名:图2.jpg)
不知道这样的效果是好是坏，希望有经验的大牛给点意见与建议，以欢迎大家共同讨论，还有那个0.02是否就约等于陀螺仪的零漂？

zerok 发表于 2011-11-23 20:18:10

不知道莫大叔会不会看到这个，如果看到了的话希望给点意见，谢谢

majianjia 发表于 2011-11-23 20:20:12

牛呀～这是什么语言？

zerok 发表于 2011-11-23 20:21:50

MATlab回复【2楼】majianjia 阿嘉^_^
-----------------------------------------------------------------------

MATlab

zerok 发表于 2011-11-23 20:25:30

有点疑问：在图中的蓝色线表示的是由角速率直接积分得到的角度，并不断累加的结果，其变化非常小，而经过卡尔曼滤波后的角度为红色线表示，感觉更接近真实情况，陀螺仪数据就是手拿着陀螺仪板来回旋转得到，旋转角度应该在正负60左右。

XIE2099 发表于 2011-11-23 22:13:35

支持一下

jade0606 发表于 2011-11-24 11:54:55

可能速度的測量有偏移,是不是需要校正

nirvanasyl 发表于 2011-11-24 16:11:37

mark

wscjun 发表于 2011-11-25 14:54:30

最近学习卡尔曼滤波中，感觉似懂非懂

ligongxiaobie 发表于 2011-11-29 19:16:43

楼主 你真牛啊 交个朋友吧

ligongxiaobie 发表于 2011-11-29 19:28:45

我最近也在弄卡曼滤波 交流一下哦不

tree888 发表于 2012-2-14 12:08:24

mark

spispispirit 发表于 2012-2-14 13:10:07

mark

yangzy09 发表于 2012-2-15 23:41:34

滤波啊滤波，头疼啊头疼

changqing 发表于 2012-2-16 20:15:02

回复【2楼】majianjia阿嘉^_^
-----------------------------------------------------------------------
MATLAB 呵呵 类似C语言

changqing 发表于 2012-2-16 20:21:57

回复【楼主位】zerok
-----------------------------------------------------------------------

楼主写的卡尔曼算法的步骤应该是正确的，但是就后验估计协方差矩阵的更新好像有不同的答案，你考虑了B阵，这在有些资料上说是控制增益，有些说是误差增益，我看最好先根据书籍将原理搞清楚了，这样写程序才会有依据，知道卡尔曼系数该怎样调节。

wangyu911310 发表于 2012-5-24 15:54:39

看完后再回来关注~
页: [1]
查看完整版本: 卡尔曼滤波