📄 kf_sins.m
字号:
%=================本程序为捷联惯导系统静基座对准卡尔曼滤波程序================
%===========================观测量为水平速度误差===========================
%==========================================================================
clear all
close all
clc
%------------------------------常量定义区----------------------------------
L=45*pi/180; %初始纬度
g0=9.78049;
g=g0*(1+0.0052884*sin(L)*sin(L)-0.0000059*sin(2*L)*sin(2*L)); %实验当地重力加速度计算
wie=0.00007272205216643039903848711535368; %地球自转角速度 单位：弧度每秒
wie_e=0;
wie_n=wie*cos(L); %导航坐标系选为当地地理坐标系 东北天方向
wie_u=wie*sin(L);
Re=6378137.0; %地球半径:1984年WGS-84全球
T_align=5*60; %对准时间为5分钟
T_samp=0.05; %惯性器件的采样时间 0.05s
number_data=T_align/T_samp; %数据个数 6000个
%--------------------------设定初始捷联矩阵-----------------------------------
%姿态角设定
yaw_angle=pi*0/180; %偏航角
pitching_angle=pi*0/180; %俯仰角
turn_angle=pi*0/180; %滚动角
%calculate of ideal strapdown_matrix from chenzhe
Cbt_idea11=cos(turn_angle)*cos(yaw_angle)-sin(turn_angle)*sin(pitching_angle)*sin(yaw_angle);
Cbt_idea12=cos(turn_angle)*sin(yaw_angle)+sin(turn_angle)*sin(pitching_angle)*cos(yaw_angle);
Cbt_idea13=-sin(turn_angle)*cos(pitching_angle);
Cbt_idea21=-cos(pitching_angle)*sin(yaw_angle);
Cbt_idea22=cos(pitching_angle)*cos(yaw_angle);
Cbt_idea23=sin(pitching_angle);
Cbt_idea31=sin(turn_angle)*cos(yaw_angle)+cos(turn_angle)*sin(pitching_angle)*sin(yaw_angle);
Cbt_idea32=sin(turn_angle)*sin(yaw_angle)-cos(turn_angle)*sin(pitching_angle)*cos(yaw_angle);
Cbt_idea33=cos(turn_angle)*cos(pitching_angle);
Cbt_idea=[Cbt_idea11 Cbt_idea12 Cbt_idea13
Cbt_idea21 Cbt_idea22 Cbt_idea23
Cbt_idea31 Cbt_idea32 Cbt_idea33];
Ctb_idea=Cbt_idea';
%------------------------------初始值的设定--------------------------------
%导航坐标系选东北天
fiee=1*pi/180; %初始误差角均为1度
fien=1*pi/180;
fieu=1*pi/180;
fpin=fopen('guanceliang.dat', 'r'); %打开文件，观测量数据（人造）
%东北天坐标系下系统的状态矩阵(连续系统）
A=zeros(10,10);
A(1,2)=2*wie_u;
A(1,4)=-g;
A(1,6)=Ctb_idea(1,1);
A(1,7)=Ctb_idea(1,2);
A(2,1)=-2*wie_u;
A(2,3)=g;
A(2,6)=Ctb_idea(2,1);
A(2,7)=Ctb_idea(2,2);
A(3,4)=wie_u;
A(3,5)=-wie_n;
A(3,8)=Ctb_idea(1,1);
A(3,9)=Ctb_idea(1,2);
A(3,10)=Ctb_idea(1,3);
A(4,3)=-wie_u;
A(4,8)=Ctb_idea(2,1);
A(4,9)=Ctb_idea(2,2);
A(4,10)=Ctb_idea(2,3);
A(5,3)=wie_n;
A(5,8)=Ctb_idea(3,1);
A(5,9)=Ctb_idea(3,2);
A(5,10)=Ctb_idea(3,3);
%观测矩阵
H=zeros(2,10);
H(1,1)=1;
H(2,2)=1;
%初始状态向量 初始状态都设为0
X=zeros(10,1);
X(3,1)=0*pi/180;
X(4,1)=0*pi/180;
X(5,1)=0*pi/180;
%初始观测值 初始观测值设为0
Z=zeros(2,1);
%初始方差阵的设置
%初始速度误差取0.1米每妙 初始失准角均为1度
%加速度计的初始偏值均取1e-4*g 陀螺的常值漂移取0.1度每小时
P=zeros(10,10);
P(1,1)=0.1^2;
P(2,2)=0.1^2;
P(3,3)=(1*pi/180)^2;
P(4,4)=(1*pi/180)^2;
P(5,5)=(1*pi/180)^2;
P(6,6)=(1e-4*g)^2;
P(7,7)=(1e-4*g)^2;
P(8,8)=(0.02*pi/3600/180)^2;
P(9,9)=(0.02*pi/3600/180)^2;
P(10,10)=(0.02*pi/3600/180)^2;
%＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝初始系统噪声误差阵＝＝＝＝＝＝＝＝＝＝＝＝＝＝%
%加速度计的随机偏差为0.5e-4*g 陀螺的随机漂移为0.05度每小时
Q=zeros(10,10);
Q(1,1)=(5e-5*g)^2;
Q(2,2)=(5e-5*g)^2;
Q(3,3)=(0.01*pi/3600/180)^2;
Q(4,4)=(0.01*pi/3600/180)^2;
Q(5,5)=(0.01*pi/3600/180)^2;
%初始观测噪声误差阵
R=zeros(2,2);
R(1,1)=0.1^2;
R(2,2)=0.1^2;
%==================状态矩阵和系统噪声阵的离散化====================%
T_discre=0.05; %离散化时间
FI=eye(10);
Qdct=zeros(10);
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
%============================== mine new1===============================
%============================ new1 end===================================
fpout_result=fopen('KF_result_state.dat','w'); %创建文件，保存滤波结果
for i=1:number_data; %kalman滤波开始
fscanf(fpin, '%g', 1);
m=fscanf(fpin, '%g', 1);
n=fscanf(fpin, '%g', 1);
Z(1,1)=m;
Z(2,1)=n;
a=fscanf(fpin, '%g', 1); %单位：角分 1度＝60角分
b=fscanf(fpin, '%g', 1);
c=fscanf(fpin, '%g', 1);
%============================== mine new2 ===============================%
%============================== new2 end ================================%
P=FI*P*FI'+Qdct;
K=P*H'*inv(H*P*H'+R);
X=FI*X+K*(Z-H*FI*X);
P=(eye(10)-K*H)*P*(eye(10)-K*H)'+K*R*K';
%估计误差的方差
ddeltav_e(i)=sqrt(P(1,1)); %速度估计误差的方差 单位 m/s
ddeltav_n(i)=sqrt(P(2,2));
dfiee(i)=sqrt(P(3,3))*180*60*60/pi; %水平失准角 单位角秒
dfien(i)=sqrt(P(4,4))*180*60*60/pi;
dfieu(i)=sqrt(P(5,5))*180*60/pi; %方位失准角 单位角分
daccle_meter_x(i)=sqrt(P(6,6))*1e+6/g; %加速度计偏置 单位 ug
daccle_meter_y(i)=sqrt(P(7,7))*1e+6/g;
dgyro_x(i)=sqrt(P(8,8))*180*3600/pi; %陀螺漂移 单位度每小时
dgyro_y(i)=sqrt(P(9,9))*180*3600/pi;
dgyro_z(i)=sqrt(P(10,10))*180*3600/pi;
% dfiee(i)=a*60-(X(3,1))*180*60*60/pi; %保存三个姿态误差角
% dfien(i)=b*60-(X(4,1))*180*60*60/pi; %水平误差角的估计误差单位 角秒
% dfieu(i)=c-(X(5,1))*180*60/pi; %方位误差角的估计误差单位 角分
fprintf(fpout_result,'%f %25.16g %25.16g %25.16g %25.16g %25.16g %25.16g %25.16g %25.16g %25.16g %25.16g\n',...
i-1,ddeltav_e(i),ddeltav_n(i),dfiee(i),dfien(i),dfieu(i),daccle_meter_x(i),daccle_meter_y(i),dgyro_x(i),dgyro_y(i),dgyro_z(i));
%保存滤波结果 即估计误差的方差
end
%----------------------------------- 画图----------------------------------
t=0.05:0.05:60*5;
%估计误差的方差曲线
figure(1)
plot(t,ddeltav_e)
xlabel('时间 /s')
ylabel('东向速度误差 /（m/s）')
grid on
figure(2)
plot(t,ddeltav_n)
xlabel('时间 /s')
ylabel('北向速度误差 /（m/s）')
grid on
figure(3)
plot(t,dfiee)
xlabel('时间 /s')
ylabel('东向失准角 /（角秒）')
grid on
figure(4)
plot(t,dfien)
xlabel('时间 /s')
ylabel('北向失准角 /（角秒）')
grid on
figure(5)
plot(t,dfieu)
xlabel('时间 /s')
ylabel('方位失准角 /（角分）')
grid on
figure(6)
plot(t,daccle_meter_x)
xlabel('时间 /s')
ylabel('x向加速度计漂移 /（ug）')
grid on
figure(7)
plot(t,daccle_meter_y)
xlabel('时间 /s')
ylabel('y向加速度计漂移 /（ug）')
grid on
figure(8)
plot(t,dgyro_x)
xlabel('时间 /s')
ylabel('x向陀螺漂移 /（度每小时）')
grid on
figure(9)
plot(t,dgyro_y)
xlabel('时间 /s')
ylabel('y向陀螺漂移 /（度每小时）')
grid on
figure(10)
plot(t,dgyro_z)
xlabel('时间 /s')
ylabel('z向陀螺漂移 /（度每小时）')
grid on
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -