simu4kf.m

自编Kalman滤波算法仿真示例程序Matlab版

function simu4kf
%卡尔曼滤波算法仿真示例

clc;

deltagyro=0.0;
deltaalpha=0.0;
deltaa=0.0;
deltav=0.0;
deltas=0.0;

s=[randn(1)*deltas;randn(1)*deltas];
v=[randn(1)*deltav;randn(1)*deltav];
alpha=randn(1)*deltaalpha;

%
P=diag([2^2,2^2,0.1^2,0.1^2,0.002^2]);
%
X=zeros(5,1);

simutime=200;
deltat=0.02;

time=0;
gyro_=sin(pi*time/100)+randn(1)*deltagyro;
a_=[10*sin(pi*time/100)+randn(1)*deltaa;10*cos(pi*time/100)+randn(1)*deltaa];
v_=v;
C_=alpha2Cbn(alpha);

for i=1:(simutime/deltat)
    time=deltat*i;
    
    gyro=sin(pi*time/100)+randn(1)*deltagyro;
    a=[10*sin(pi*time/100)+randn(1)*deltaa;10*cos(pi*time/100)+randn(1)*deltaa];
    
    alpha=alpha+(gyro_+gyro)*deltat/2;
    C=alpha2Cbn(alpha);
    v=v+(C_*a_+C*a)*deltat/2;
    s=s+(v_+v)*deltat/2;

    
    
    tsva(:,i)=[time;s;v;alpha];
    
    gyro_=gyro;
    a_=a;
    v_=v;
    C_=C;
end

save tsva.mat tsva
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;
load tsva
delete tsva.mat

deltagyro=0.001;
deltaalpha=0.002;
deltaa=0.01;
deltav=0.1;
deltas=2;

s=[randn(1)*deltas;randn(1)*deltas];
v=[randn(1)*deltav;randn(1)*deltav];
alpha=randn(1)*deltaalpha;

%
P=diag([2^2,2^2,0.1^2,0.1^2,0.002^2]);
%
X=zeros(5,1);

simutime=200;
deltat=0.02;

time=0;
gyro_=sin(pi*time/100)+randn(1)*deltagyro;
a_=[10*sin(pi*time/100)+randn(1)*deltaa;10*cos(pi*time/100)+randn(1)*deltaa];
v_=v;
C_=alpha2Cbn(alpha);

j=1;
for i=1:(simutime/deltat)
    time=deltat*i;
    
    gyro=sin(pi*time/100)+randn(1)*deltagyro;
    a=[10*sin(pi*time/100)+randn(1)*deltaa;10*cos(pi*time/100)+randn(1)*deltaa];
    
    alpha=alpha+(gyro_+gyro)*deltat/2;
    C=alpha2Cbn(alpha);
    v=v+(C_*a_+C*a)*deltat/2;
    s=s+(v_+v)*deltat/2;
    deltatsva(:,i)=[time;s;v;alpha];
    
    
    if mod(i,50)==0 & mod(j,2)==0
    a_n=C*a;
    F=[0,0,1,0,0;0,0,0,1,0;0,0,0,0,-a_n(2);0,0,0,0,a_n(1);0,0,0,0,0];
    G=[0,0,0;0,0,0;C,[0;0];0,0,1];
    H=[0,0,1,0,0;0,0,0,1,0];
    %H=[1,0,0,0,0;0,1,0,0,0];
    
    Q=diag([deltaa^2,deltaa^2,deltagyro^2]);
    R=diag([deltav^2,deltav^2]);
    
    [Phi,GQGk]=Discretize(F,G,Q,deltat*50);
    
    Z=v-tsva(4:5,i)+[randn(1)*deltav;randn(1)*deltav];
    %Z=s-tsva(2:3,i)+[randn(1)*deltas;randn(1)*deltas];
    
    [X,P]=KalmanFilter(X,Z,P,Phi,H,GQGk,R);
        
    deltatsva(2:6,i)=deltatsva(2:6,i)-X;
    
    s=deltatsva(2:3,i);
    v=deltatsva(4:5,i);
    alpha=deltatsva(6,i);
    X=zeros(5,1);
    j=j+1;
    end
    if mod(i,50)==0 & mod(j+1,2)==0
    a_n=C*a;
    F=[0,0,1,0,0;0,0,0,1,0;0,0,0,0,-a_n(2);0,0,0,0,a_n(1);0,0,0,0,0];
    G=[0,0,0;0,0,0;C,[0;0];0,0,1];
    %H=[0,0,1,0,0;0,0,0,1,0];
    H=[1,0,0,0,0;0,1,0,0,0];
    
    Q=diag([deltaa^2,deltaa^2,deltagyro^2]);
    R=diag([deltav^2,deltav^2]);
    
    [Phi,GQGk]=Discretize(F,G,Q,deltat*50);
    
    %Z=v-tsva(4:5,i)+[randn(1)*deltav;randn(1)*deltav];
    Z=s-tsva(2:3,i)+[randn(1)*deltas;randn(1)*deltas];
    
    [X,P]=KalmanFilter(X,Z,P,Phi,H,GQGk,R);
        
    deltatsva(2:6,i)=deltatsva(2:6,i)-X;
    
    s=deltatsva(2:3,i);
    v=deltatsva(4:5,i);
    alpha=deltatsva(6,i);
    X=zeros(5,1);
    j=j+1;
    end
    gyro_=gyro;
    a_=a;
    v_=v;
    C_=C;
end
deltatsva(2:6,:)=deltatsva(2:6,:)-tsva(2:6,:);

hold on;
plot(deltatsva(1,:),deltatsva(2,:),'r');
plot(deltatsva(1,:),deltatsva(3,:),'g');
hold off;
grid on;


function Cbn=alpha2Cbn(alpha)

Cbn=[cos(alpha),-sin(alpha);sin(alpha),cos(alpha)];