📄 kalmanfilter.m
字号:
%%%%%%%%%%%%%%%%%卡尔曼滤波程序%%%%%%%%%%%%%
clear all
close all
clc
deg_rad=pi/180; %由度转化成弧度
rad_deg=180/pi; %由弧度转化成度
%-----------------------常值定义区----------------------
Omega_ie=0.00007272205216643040; %地球自转角速度 单位：弧度每妙
g0=9.78;
Wiee = [0 0 Omega_ie]'; %地球速率在地球系中的投影
Re=6378245.0; %地球长轴 《惯性导航系统》 P28
e=1/298.3;
fid_IMUout=fopen('IMUout.txt','r'); %读取惯性器件的输出 无误差
[AllDataIMUout NumofAllDataIMUout]=fscanf(fid_IMUout,'%g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g',[17 inf]);
AllDataIMUout=AllDataIMUout';
NumofEachDataIMUout=fix(NumofAllDataIMUout/17);
TimeIMUout=AllDataIMUout(:,1);
pitch=AllDataIMUout(:,11); %俯仰角（向上为正）
roll=AllDataIMUout(:,12); %滚转角（向右为正）
head=-AllDataIMUout(:,13); %偏航角（偏西为正）
%-------------------从文件中读取数据----------------------
fread_SINS=fopen('Navigation.txt','r'); %读取惯导输出
[AllData NumofAllData]=fscanf(fread_SINS,'%f %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g ',[16 inf]);
AllData=AllData';
NumofEachData=fix(NumofAllData/16);
TimeSINS=AllData(:,1);
longitude=AllData(:,2); %经度 单位：弧度
latitude=AllData(:,3); %纬度 单位：弧度
High=AllData(:,4); %高度 单位：米
Vn_err=AllData(:,5);
Vu_err=AllData(:,6);
Ve_err=AllData(:,7);
pitch_err=AllData(:,8); %俯仰角（向上为正）
roll_err=AllData(:,9); %滚转角（向右为正）
head_err=AllData(:,10); %偏航角（偏西为正
omigax=AllData(:,11); %陀螺输出
omigay=AllData(:,12);
omigaz=AllData(:,13);
fb_x=AllData(:,14); %比力
fb_y=AllData(:,15); %
fb_z=AllData(:,16); %
fread_GPS=fopen('GPSout.txt','r'); %读取速度和位置信息 带有GPS误差
[GPSData NumofData]=fscanf(fread_GPS,'%g %g %g %g %g %g %g',[7 inf]);
GPSData=GPSData';
NumofGPSData=fix(NumofData/7);
TimeGPS=GPSData(:,1);
GPS_lon=GPSData(:,2);
GPS_lat=GPSData(:,3);
GPS_High=GPSData(:,4);
GPS_Vn=GPSData(:,5);
GPS_Vu=GPSData(:,6);
GPS_Ve=GPSData(:,7);
T_SINS=TimeSINS(2)-TimeSINS(1); %惯导程序的解算周期
TimeAll=(NumofEachData-1)*T_SINS;
T_GPS=TimeGPS(2)-TimeGPS(1); %GPS的输出频率
numb_KF=TimeAll/T_GPS;
NUM=fix((NumofEachData)/(NumofGPSData));
%观测矩阵的初始化
H=zeros(3,12);
H(1,1)=1;
H(2,2)=1;
H(3,3)=1;
%初始状态向量 初始状态都设为0
X=zeros(12,1); %12个状态分别为 速度误差 姿态角误差 惯性器件误差
X(4,1)=0*pi/180;
X(5,1)=0*pi/180;
X(6,1)=0*pi/180;
%初始方差阵的设置
P=zeros(12,12);
P(1,1)=1^2;
P(2,2)=1^2;
P(3,3)=1^2;
P(4,4)=(1*pi/180)^2;
P(5,5)=(1*pi/180)^2;
P(6,6)=(1*pi/180)^2;
P(7,7)=(1e-4*g0)^2;
P(8,8)=(1e-4*g0)^2;
P(9,9)=(1e-4*g0)^2;
P(10,10)=(0.2*pi/3600/180)^2;
P(11,11)=(0.2*pi/3600/180)^2;
P(12,12)=(0.2*pi/3600/180)^2;
%P=P;
%初始系统噪声误差阵
Q=zeros(12,12);
Q(1,1)=(5e-5*g0)^2; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q(2,2)=(5e-5*g0)^2;
Q(3,3)=(5e-5*g0)^2; %系统噪声方差阵 和惯性器件的误差有关
Q(4,4)=(0.2*pi/3600/180)^2;
Q(5,5)=(0.2*pi/3600/180)^2;
Q(6,6)=(0.2*pi/3600/180)^2;
%Q=Q;
%初始观测噪声误差阵
R=zeros(3,3);
R(1,1)=5^2; %测量噪声方差阵 和GPS的测量精度有关
R(2,2)=5^2;
R(3,3)=5^2;
%R=5*R;
for ii=1:numb_KF
KFii=1+(ii-1)*NUM;
A=zeros(12,12);
M_roll=[1 0 0;
0 cos(roll_err(KFii)) sin(roll_err(KFii));
0 -sin(roll_err(KFii)) cos(roll_err(KFii))];
M_pitch=[cos(pitch_err(KFii)) sin(pitch_err(KFii)) 0;
-sin(pitch_err(KFii)) cos(pitch_err(KFii)) 0;
0 0 1];
M_head=[cos(head_err(KFii)) 0 -sin(head_err(KFii));
0 1 0;
sin(head_err(KFii)) 0 cos(head_err(KFii))];
KF_Tbn_err=M_roll*M_pitch*M_head; %地理系：北天东方向 弹体系：前上右 转序：head-pitch-roll
KF_Tnb_err=KF_Tbn_err';
KF_fb(1,1)=fb_x(KFii);
KF_fb(2,1)=fb_y(KFii);
KF_fb(3,1)=fb_z(KFii);
L=GPS_lat(ii);
RM0=Re*(1.0-2.0*e+3.0*e*sin(L)^2)+GPS_High(ii); %
RN0=Re*(1.0+e*sin(L)^2)+GPS_High(ii);
Re_ideal=Re+GPS_High(ii);
KF_Vn=GPS_Vn(ii);
KF_Vu=GPS_Vu(ii);
KF_Ve=GPS_Ve(ii);
KF_lat=GPS_lat(ii);
KF_lon=GPS_lon(ii);
KF_High=GPS_High(ii);
KF_fn=KF_Tnb_err*KF_fb;
A(1,1)=-KF_Vu/RM0;
A(1,2)=-KF_Vn/RM0;
% A(1,3)=-(2*Omega_ie*sin(L)+KF_Ve*tan(L)/Re_ideal); %%%%%%%%%%%%%%%%%%%%
A(1,3)=-2*(Omega_ie*sin(L)+KF_Ve*tan(L)/RN0);
A(1,5)=-KF_fn(3,1);
A(1,6)=KF_fn(2,1);
A(2,1)=2*KF_Vn/RM0;
% A(2,3)=2*Omega_ie*cos(L)+KF_Ve/Re_ideal;
A(2,3)=2*(Omega_ie*cos(L)+KF_Ve/RN0);
A(2,4)=KF_fn(3,1);
A(2,6)=-KF_fn(1,1);
A(3,1)=2*Omega_ie*sin(L)+KF_Ve/RN0*tan(L);
A(3,2)=-(2*Omega_ie*cos(L)+KF_Ve/RN0);
A(3,3)=(KF_Vn*tan(L))/RN0-KF_Vu/RN0;
A(3,4)=-KF_fn(2,1);
A(3,5)=KF_fn(1,1);
A(4,3)=1.0/RN0;
A(4,5)=-KF_Vn/RM0;
A(4,6)=-(Omega_ie*sin(L)+(KF_Ve*tan(L))/RN0);
A(5,3)=tan(L)/RN0;
A(5,4)=KF_Vn/RM0;
A(5,6)=Omega_ie*cos(L)+KF_Ve/RN0;
A(6,1)=-1.0/RM0;
A(6,4)=Omega_ie*sin(L)+KF_Ve*tan(L)/RN0;
A(6,5)=-(Omega_ie*cos(L)+KF_Ve/RN0);
A(1,7)=KF_Tnb_err(1,1);
A(1,8)=KF_Tnb_err(1,2); %12个状态分别为 速度误差 姿态角误差 惯性器件误差
A(1,9)=KF_Tnb_err(1,3);
A(2,7)=KF_Tnb_err(2,1);
A(2,8)=KF_Tnb_err(2,2);
A(2,9)=KF_Tnb_err(2,3);
A(3,7)=KF_Tnb_err(3,1);
A(3,8)=KF_Tnb_err(3,2);
A(3,9)=KF_Tnb_err(3,3);
A(4,10)=KF_Tnb_err(1,1);
A(4,11)=KF_Tnb_err(1,2);
A(4,12)=KF_Tnb_err(1,3);
A(5,10)=KF_Tnb_err(2,1);
A(5,11)=KF_Tnb_err(2,2);
A(5,12)=KF_Tnb_err(2,3);
A(6,10)=KF_Tnb_err(3,1);
A(6,11)=KF_Tnb_err(3,2);
A(6,12)=KF_Tnb_err(3,3);
%==================状态矩阵和系统噪声阵的离散化====================%
T_discre=T_GPS; %离散化时间
FI=eye(12);
Qdct=zeros(12);
temp2=1;
M{1}=Q;
for j=1:10
temp2=temp2*j;
FI=FI+T_discre^j*(A)^j/temp2;
if(j~=1)
M{j}=A*M{j-1}+(A*M{j-1})';
end
Qdct =Qdct+T_discre^j*M{j}/temp2;
end
% 读取观测数据进行卡尔曼滤波
Z(1,1)=Vn_err(KFii)-GPS_Vn(ii);
Z(2,1)=Vu_err(KFii)-GPS_Vu(ii);
Z(3,1)=Ve_err(KFii)-GPS_Ve(ii);
%进行一次递推
P=FI*P*FI'+Qdct;
K=P*H'*inv(H*P*H'+R);
ZZ(ii,:)=(Z-H*FI*X)';
KK(:,:,ii)=K;
X=FI*X+K*(Z-H*FI*X);
P=(eye(12)-K*H)*P*(eye(12)-K*H)'+K*R*K';
KF_dVn(ii)=X(1,1);
KF_dVu(ii)=X(2,1);
KF_dVe(ii)=X(3,1);
KF_fien(ii)=X(4,1);
KF_fieu(ii)=X(5,1);
KF_fiee(ii)=X(6,1);
%-----------------用滤波的结果对姿态矩阵进行校正------------------------
Tbn(1,1)=cos(pitch_err(KFii))*cos(head_err(KFii)); %地理坐标系 北天东
Tbn(1,2)=sin(pitch_err(KFii)); %载体坐标系 前上右
Tbn(1,3)=-cos(pitch_err(KFii))*sin(head_err(KFii)); %旋转顺序 head-pitch-roll
Tbn(2,1)=sin(roll_err(KFii))*sin(head_err(KFii))-sin(pitch_err(KFii))*cos(roll_err(KFii))*cos(head_err(KFii));
Tbn(2,2)=cos(roll_err(KFii))*cos(pitch_err(KFii));
Tbn(2,3)=cos(roll_err(KFii))*sin(head_err(KFii))*sin(pitch_err(KFii))+sin(roll_err(KFii))*cos(head_err(KFii));
Tbn(3,1)=cos(roll_err(KFii))*sin(head_err(KFii))+sin(roll_err(KFii))*sin(pitch_err(KFii))*cos(head_err(KFii));
Tbn(3,2)=-sin(roll_err(KFii))*cos(pitch_err(KFii));
Tbn(3,3)=-sin(roll_err(KFii))*sin(head_err(KFii))*sin(pitch_err(KFii))+cos(roll_err(KFii))*cos(head_err(KFii));
Tnb=Tbn';
Tnn11=[1 -X(6,1) X(5,1);
X(6,1) 1 -X(4,1);
-X(5,1) X(4,1) 1];
%用滤波的结果对有较大误差的姿态角进行校正
Tnb_modify=Tnn11*Tnb;
Tbn_modify=Tnb_modify';
%解算姿态角
flag_pitch=asin(Tnb_modify(2,1));
flag_roll=atan(-Tnb_modify(2,3)/Tnb_modify(2,2));
flag_head=atan(-Tnb_modify(3,1)/Tnb_modify(1,1));
if(Tnb_modify(2,2)<0)
if(flag_roll<0)
flag_roll=flag_roll+pi;
else
flag_roll=flag_roll-pi;
end
end
% 偏航角范围 -180度——180度 北偏西为正
if(Tnb_modify(1,1)<0)
if(flag_head<0)
flag_head=flag_head+pi;
else
flag_head=flag_head-pi;
end
end
pitch_modify(ii)=flag_pitch;
head_modify(ii)=flag_head;
roll_modify(ii)=flag_roll;
dpitch_modify(ii)=(pitch_modify(ii)-pitch(KFii))*rad_deg*60;
dhead_modify(ii)=(head_modify(ii)-head(KFii))*rad_deg*60; %单位：角分
droll_modify(ii)=(roll_modify(ii)-roll(KFii))*rad_deg*60;
end;
% -------------------------画图------------------------------
t=T_GPS:T_GPS:TimeAll;
figure(1)
plot(t,dpitch_modify,t,-18,'r',t,18,'r')
xlabel('时间（t）')
ylabel('俯仰角的修正误差（角分）')
legend('收敛曲线','最后收敛值±18角分')
figure(2)
plot(t,dhead_modify,t,-18,'r',t,18,'r')
xlabel('时间（t）')
ylabel('航向角的修正误差（角分）')
legend('收敛曲线','最后收敛值±18角分')
figure(3)
plot(t,droll_modify,t,-18,'r',t,18,'r')
xlabel('时间（t）')
ylabel('滚转角的修正误差（角分）')
legend('收敛曲线','最后收敛值±18角分')
fclose(fid_IMUout);
fclose(fread_SINS);
fclose(fread_GPS);
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -