[求助]Matlab仿真的卡尔曼滤波器工作不正常。
下面是我用Matlab写的加速度计和陀螺仪融合的卡尔曼滤波器仿真代码，是根据这篇论文写的，但是出来的曲线跟陀螺仪的计算角度曲线是重合，请问一下是什么问题？close all;
clear all;
fid = fopen('./2014-11-11_03_13_25.dat', 'rb');
fseek(fid,0,'eof');
fsize = ftell(fid);
fseek(fid,0,'bof');
= fread(fid, 29, 'uchar');
%卡尔曼滤波相关变量初始化
T = 0.001;
Q = ;
delta = 0;
% r_k = 0.05;
X_0 = ;
A = ;
B = [(1-delta)*T;delta];
C = ;%系统输出方程中的输出矩阵
% P_k_1 = 0;
R_k = ;
X_k_k = X_0;
I = eye(2);
P_k_k = ;
%获取文件数据
data_size = int32((fsize -29)/12);
Accel_factor = 4096.0;
Gyro_factor = 65.5;
accel_x = zeros(1,data_size);
accel_y = zeros(1,data_size);
accel_z = zeros(1,data_size);
gyro_x = zeros(1,data_size);
gyro_y = zeros(1,data_size);
gyro_z = zeros(1,data_size);
Aax = zeros(1,data_size);
Aay = zeros(1,data_size);
Aaz = zeros(1,data_size);
Agx = zeros(1,data_size);
Agy = zeros(1,data_size);
Agz = zeros(1,data_size);
Af_k_1 = zeros(1,data_size);
Af_k_2 = zeros(1,data_size);
for i = 1:1:data_size
= fread(fid, 2, 'uchar');
= fread(fid, 2, 'uchar');
= fread(fid, 2, 'uchar');
= fread(fid, 2, 'uchar');
= fread(fid, 2, 'uchar');
= fread(fid, 2, 'uchar');
accel_x(i) = uint16Toint16(Accel_X(1) * 256 + Accel_X(2)) / Accel_factor;
accel_y(i) = uint16Toint16(Accel_Y(1) * 256 + Accel_Y(2)) / Accel_factor;
accel_z(i) = uint16Toint16(Accel_Z(1) * 256 + Accel_Z(2)) / Accel_factor;
gyro_x(i)=uint16Toint16(Gyro_X(1)* 256 + Gyro_X(2) ) / Gyro_factor ;
gyro_y(i)=uint16Toint16(Gyro_Y(1)* 256 + Gyro_Y(2) ) / Gyro_factor ;
gyro_z(i)=uint16Toint16(Gyro_Z(1)* 256 + Gyro_Z(2) ) / Gyro_factor ;
end
%平滑滤波
% accel_x=smooth(accel_x,50, 'moving');
% accel_y=smooth(accel_y,50, 'moving');
% accel_z=smooth(accel_z,50, 'moving');
% gyro_x =smooth(gyro_x, 50, 'moving');
% gyro_y =smooth(gyro_y, 50, 'moving');
% gyro_z =smooth(gyro_z, 50, 'moving');
for i=1:1:data_size
if(i==1)
%加速度计得到的角度
Aax(i) = -atan2(accel_x(i), accel_z(i));
Aay(i) = atan2(accel_y(i), accel_z(i));
Aaz(i) = atan2(accel_x(i), accel_y(i));
%陀螺仪得到的角度
Agx(i) = Aay(1);
Agy(i) = Aax(1);
Agz(i) = Aaz(1);
else
%加速度计得到的角度
Aax(i) = -atan2(accel_x(i), accel_z(i));
Aay(i) = atan2(accel_y(i), accel_z(i));
Aaz(i) = atan2(accel_x(i), accel_y(i));
%陀螺仪得到的角度
Agx(i) = Agx(i-1) + gyro_x(i) * T * pi / 180.0;
Agy(i) = Agy(i-1) + gyro_y(i) * T * pi / 180.0;
Agz(i) = Agz(i-1) + gyro_z(i) * T * pi / 180.0;
end
Ag_k = Aax(i);%k时刻加速度计输出的姿态角
Aw_k = Agy(i);%k时刻陀螺仪输出的姿态角
error_k = Aw_k - Ag_k; %姿态角在k时刻的偏差
if(i==1)
u_k = 0;
else
u_k = gyro_y(i-1) * T * pi / 180.0;
end
X_k_1_k_1 = X_k_k;
X_k_k_1 = A * X_k_1_k_1+ B * u_k;
P_k_1_k_1 = P_k_k;
P_k_k_1 = A * P_k_1_k_1 * (A') + Q;
%计算卡尔曼增益
%P_k_1是k-1时刻的滤波协方差
K_k = P_k_k_1 * (C') / (C * P_k_k_1 * (C') + R_k);
X_k = ;
Y_k = C * X_k;
X_k_k = X_k_k_1 + K_k * (Y_k - C * X_k_k_1);
P_k_k = (I - K_k * C) * P_k_k_1;
Af_k_1(i) = X_k_k(1);
Af_k_2(i) = X_k_k(2);
% 系统输出方程
% Y_k = C * X_k;
% 对姿态角预测值进行修正,Af_k为k时刻姿势角的最终输出值
% Af_k(i) = Aw_k + K_k * error_k;
% 计算下一步滤波算法中的滤波协方差
% P_k_1 = P_k_1 - K_k * C * P_k_1;
end
plot(Aax, 'r');
hold on;
plot(Agy, 'g');
hold on;
plot(Af_k_1, 'b');
hold on;
plot(Af_k_2, 'c');
hold on;
fclose(fid);
accel_x，accel_y和accel_z是加速度计的三轴数据。
gyro_x，gyro_y和gyro_z是陀螺仪的输出角速度数据。
仿真曲线如下：
其中红线是加速度计计算得到的角度，蓝线是卡尔曼滤波器输出的角度，绿色是陀螺仪积分计算得到的角度数据（绿线与蓝线重合了，所以是看到蓝线） 补充一下，浅蓝色线是状态矩阵X_k_k中的error_k曲线
页:
[1]