姿态求解，卡尔曼算法讨论，附程序，效果图

zerok

卡尔曼滤波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([0 20 0 20]);
xlabel('time');
ylabel('yp position');
title('the initial track of movement');


(原文件名:初始轨迹.jpg)

% add white gauss noise
a=0.5*randn(1,200);
s=y+a;            %data+noise
figure(2);
plot(t,s,'-');
axis([0 20 0 20]);
xlabel('time');
ylabel('yp position');
title('the track of movement with noise');
save initial_track s



(原文件名:加入高斯白噪声后的轨迹.jpg)

% kalman filtering

load initial_track  s; % 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=[yp(n);yv(n)];
% error deviation caused by the random acceleration
% known data
Y=zeros(2,200);
Y0=[0;1];         %系统状态初始值
Y(:,1)=Y0;
A=[1,T;0,1];      %系统状态转移矩阵     
B=[1/2*(T)^2 T]'; %噪声输入矩阵
H=[1 0];          %观测阵

C0=[0,0;0,1];     %cov 初始值
C=[C0 zeros(2,2*199)];
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,:);
plot(t,yp,'-');
axis([0 20 0 20]);
xlabel('time');
ylabel('yp position');
title('the track after kalman filtering');


(原文件名:加入噪声并卡尔曼滤波后波形.jpg)


% 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');   


(原文件名:随机误差对角速率的影响.jpg)

zerok

这个是在网上找到的，有个疑问：这个程序采用的模型是什么样的呢？

zerok

个人对以上程序的理解：上面程序是对陀螺仪数据的一个预测与修正，其中陀螺仪的角速率v始终为0.5，共采集200个点，步长为0.1；y表示的是陀螺仪转过的实际角度，其初始值为0。Y包含陀螺仪的角速率(yv)，以及角度(yp)；采用的系统模型为yp(k)=yp(k-1)+yv(k-1)T+BpWp(k-1),yv(k)=yv(k-1)+BvWv(k-1)；其中W为噪声。观测值为S=（s,sv）'，其中s即加入白噪声后的角度，如图2所示,sv为0,观测模型为：S(k)=HY(k)+V(k)，其中V(k)为观测噪声。不知正确与否，欢迎讨论。

nirvanasyl

MARK

diaojiandtj

学习一下

newhand1991

好东西！

g921002

不知道好在哪裡。

Kalman filter 重點是系統模型和觀測模型(及噪訊協方差)的建立。
這兩個建立或假設錯誤，基本上就是garbage in garbage out(GIGO)。

上面做的東西基本上就是LPF，一點意義也沒有。

shadowsnow

根本看不懂啊，现在才发现当初不听课的后果