姿态求解，卡尔曼算法讨论
小弟最近也在做姿态检测相关的东西，在姿态检测的时候用的是RPY角原理，硬件是三轴加速度，三轴磁场和三轴陀螺仪。在解算姿态数据的时候，利用加速度传感器和磁场传感器的数据分别可以推出两个姿态角：加速度推出俯仰角和倾斜角；磁场传感器推出航向角和俯仰角，计算公式如图所示，各位看看对否？http://cache.amobbs.com/bbs_upload782111/files_47/ourdev_696080EEIGQL.jpg
(原文件名:姿态角求解公式.jpg)
如上述公式无误，那么得到的姿态角里面有两个俯仰角，在算法里面需要对其进行简单的融合，最后得到三个唯一的姿态角。在讲该姿态角与陀螺仪的数据进行融合。在坛子里看到很多利用卡尔曼滤波器进行数据融合的，小弟仅谈谈个人对卡尔曼滤波的理解：卡尔曼滤波主要是对数据进行一个预测，然后根据观测值(及传感器实际数据)对数据进行一个修正，不知道这样理解对否？问题在于卡尔曼滤波对数据进行预测，其预测的对象数据是陀螺仪得出的姿态数据(数据A)，还是加速度与磁场得出的姿态数据(数据B)，同时观测值是取自于A还是B？个人的理解是预测对象是A，观测值是B。不知道各位理解如何？欢迎讨论、指正。 欢迎各位拍砖哈 还有一个问题就是：在对数据AB进行融合的时候，具体如何实现？
小弟的想法：X=nXa+(1-n)Xb;权值n，根据对数据预测值与实测观测值只差定，各位觉得如何呢？ 有经验的给点意见吧，谢谢./emotion/em038.gif./emotion/em038.gif./emotion/em038.gif./emotion/em038.gif 个人认为预测和观测都是针对A的,因为所谓的卡尔曼滤波就是采用递归的方法解决离散数据线性滤波问题。看过一段时间的卡尔曼滤波也是云里雾里的，不过感觉它其实也就做了两件事：（1），利用预测值及当前测量值校正。（2），利用当前校正值对下一时刻进行预测；卡尔曼这东西真是不了解啊，特别是那个计算协方差的步骤不知道有什么作用！！ 回复【4楼】thisjoy
-----------------------------------------------------------------------
卡尔曼滤波的准则函数就是使得器协方差取最小值。 标记，回去验证一下这个公式。楼主的公式出自那本书 卡尔曼滤波MATlab程序：
% modeling data
% sample number N=200 points
% sample time T=0.1s
t=0:0.1:20-0.1;
T=0.1; % sample time
v=0.5; % initial velocity
y=ones(1,200);
y(1)=0;
for n=2:200;
y(n)=y(n-1)+v*T;
end
figure(1);
plot(t,y,'-');
axis();
xlabel('time');
ylabel('yp position');
title('the initial track of movement');
% add white gauss noise
a=0.5*randn(1,200);
s=y+a; %data+noise
figure(2);
plot(t,s,'-');
axis();
xlabel('time');
ylabel('yp position');
title('the track of movement with noise');
save initial_track s
load initial_tracks; % y:initial data,s:data with noise
T=0.1;
% yp denotes the sample value of position
% yv denotes the sample value of velocity
% Y=;
% error deviation caused by the random acceleration
% known data
Y=zeros(2,200);
Y0=; %系统状态初始值
Y(:,1)=Y0;
A=; %系统状态转移矩阵
B='; %噪声输入矩阵
H=; %观测阵
C0=; %cov 初始值
C=;
Q=(0.25)^2; %过程噪声
R=(0.25)^2; %观测噪声
% kalman algorithm ieration
for n=1:200
i=(n-1)*2+1;
K=C(:,i:i+1)*H'*inv(H*C(:,i:i+1)*H'+R);
Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n));
Y(:,n+1)=A*Y(:,n);
C(:,i:i+1)=(eye(2,2)-K*H)*C(:,i:i+1);
C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B';
end
% the diagram of position after filtering
figure(3)
t=0:0.1:20;
yp=Y(1,:);A
plot(t,yp,'-');
axis();
xlabel('time');
ylabel('yp position');
title('the track after kalman filtering');
% the diagram of velocity after filtering
figure(4)
yv=Y(2,:);
plot(t,yv,'-');
xlabel('time');
ylabel('yv velocity');
title('the velocity caused by random acceleration'); 这个公式验证感觉不太准确，比如横滚90度，逐渐增加仰角，可以发现，横滚角变小了，估计没有能直接反映欧拉角的公式。一个思路是将g通过姿态四元数变换到机体坐标系，然后与测得值一起解算姿态四元数，算出四元数了就能求出欧拉角，关键是我还不会中间的计算过程，呵呵 MARK 7LOU MARK 我的LZ的理解是正确的。
其预测的对象数据是陀螺仪得出的姿态数据(数据A)，还是加速度与磁场得出的姿态数据(数据B)，同时观测值是取自于A还是B？个人的理解是预测对象是A，观测值是B。
因为陀螺仪的数据时短期精确而且不受动作加速度和磁场干扰的影响的，陀螺仪作为主线，另外两个传感器数据作为辅助来校正它。 回复【7楼】zerok
-----------------------------------------------------------------------
可能以后要用到这个程序，先收了。。。 马克一下，明天起来在试试。 一头雾水，
最好你的地點沒有磁傾角的問題....
感谢楼主分享，本人也在学习卡尔曼滤波的程序，希望大家一起交流一下 这个问题问的太好了，我也正在想这个问题，卡尔曼到底处理的是什么数据，这个飞控程序到底要怎样运作呢，滤波姿态控制，哎，不知道到底是怎么运作的，坐等大神回答 预测值应该当前的姿态信息吧，加速度计和电子罗盘都是测量值。用测量信息与预测信息对比求误差，再根据误差校正当前的姿态信息。
页:
[1]