扩展卡尔曼滤波(EKF)应用于GPS-INS组合导航

clear all;

%% 惯性-GPS组合导航模型参数初始化

we = 360/24/60/60*pi/180; %地球自转角速度，弧度/s

psi = 10*pi/180; %psi角度/ 弧度

Tge = 0.12;

Tgn = 0.10;

Tgz = 0.10; %这三个参数的含义详见参考文献

sigma_ge=1;

sigma_gn=1;

sigma_gz=1;

%% 连续空间系统状态方程

% X_dot(t) = A(t)*X(t) + B(t)*W(t)

A=[0 we*sin(psi) -we*cos(psi) 1 0 0 1 0 0;

-we*sin(psi) 0 0 0 1 0 0 1 0;

we*cos(psi) 0 0 0 0 1 0 0 1;

0 0 0 -1/Tge 0 0 0 0 0;

0 0 0 0 -1/Tgn 0 0 0 0;

0 0 0 0 0 -1/Tgz 0 0 0;

0 0 0 0 0 0 0 0 0;

0 0 0 0 0 0 0 0 0;

0 0 0 0 0 0 0 0 0;]; %状态转移矩阵

B=[0 0 0 sigma_ge*sqrt(2/Tge) 0 0 0 0 0;

0 0 0 0 sigma_gn*sqrt(2/Tgn) 0 0 0 0;

0 0 0 0 0 sigma_gz*sqrt(2/Tgz) 0 0 0;]';%输入控制矩阵%% 转化为离散时间系统状态方程

% X(k+1) = F*X(k) + G*W(k)

T = 0.1;

[F,G]=c2d(A,B,T);

H=[1 0 0 0 0 0 0 0 0;

0 -sec(psi) 0 0 0 0 0 0 0;];%观测矩阵

%% 卡尔曼滤波器参数初始化

t=0:T:50-T;

length=size(t,2);

y=zeros(2,length);

Q=0.5^2*eye(3); %系统噪声协方差

R=0.25^2*eye(2); %测量噪声协方差

y(1,:)=2*sin(pi*t*0.5);

y(2,:)=2*cos(pi*t*0.5);

Z=y+sqrt(R)*randn(2,length); %生成的含有噪声的假定观测值，2维X=zeros(9,length); %状态估计值，9维

X(:,1)=[0,0,0,0,0,0,0,0,0]'; %状态估计初始值设定

P=eye(9); %状态估计协方差

%% 卡尔曼滤波算法迭代过程

for n=2:length

X(:,n)=F*X(:,n-1);

P=F*P*F'+ G*Q*G';

Kg=P*H'/(H*P*H'+R);

X(:,n)=X(:,n)+Kg*(Z(:,n)-H*X(:,n));

P=(eye(9,9)-Kg*H)*P;

end

%% 绘图代码

figure(1)

plot(y(1,:))

hold on;

plot(y(2,:))

hold off;

title('理想的观测量');

figure(2)

plot(Z(1,:))

hold on;

plot(Z(2,:))

hold off;

title('带有噪声的观测量'); figure(3)

plot(X(1,:))

hold on;

plot(X(2,:))

hold off;

title('滤波后的观测量');