?? kf_sins.m
字號(hào):
%=================本程序?yàn)榻萋?lián)慣導(dǎo)系統(tǒng)靜基座對(duì)準(zhǔn)卡爾曼濾波程序================
%===========================觀測(cè)量為水平速度誤差===========================
%==========================================================================
clear all
close all
clc
%------------------------------常量定義區(qū)----------------------------------
L=45*pi/180; %初始緯度
g0=9.78049;
g=g0*(1+0.0052884*sin(L)*sin(L)-0.0000059*sin(2*L)*sin(2*L)); %實(shí)驗(yàn)當(dāng)?shù)刂亓铀俣扔?jì)算
wie=0.00007272205216643039903848711535368; %地球自轉(zhuǎn)角速度 單位：弧度每秒
wie_e=0;
wie_n=wie*cos(L); %導(dǎo)航坐標(biāo)系選為當(dāng)?shù)氐乩碜鴺?biāo)系 東北天方向
wie_u=wie*sin(L);
Re=6378137.0; %地球半徑:1984年WGS-84全球
T_align=5*60; %對(duì)準(zhǔn)時(shí)間為5分鐘
T_samp=0.05; %慣性器件的采樣時(shí)間 0.05s
number_data=T_align/T_samp; %數(shù)據(jù)個(gè)數(shù) 6000個(gè)
%--------------------------設(shè)定初始捷聯(lián)矩陣-----------------------------------
%姿態(tài)角設(shè)定
yaw_angle=pi*0/180; %偏航角
pitching_angle=pi*0/180; %俯仰角
turn_angle=pi*0/180; %滾動(dòng)角
%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';
%------------------------------初始值的設(shè)定--------------------------------
%導(dǎo)航坐標(biāo)系選東北天
fiee=1*pi/180; %初始誤差角均為1度
fien=1*pi/180;
fieu=1*pi/180;
fpin=fopen('guanceliang.dat', 'r'); %打開(kāi)文件，觀測(cè)量數(shù)據(jù)（人造）
%東北天坐標(biāo)系下系統(tǒng)的狀態(tài)矩陣(連續(xù)系統(tǒng)）
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);
%觀測(cè)矩陣
H=zeros(2,10);
H(1,1)=1;
H(2,2)=1;
%初始狀態(tài)向量 初始狀態(tài)都設(shè)為0
X=zeros(10,1);
X(3,1)=0*pi/180;
X(4,1)=0*pi/180;
X(5,1)=0*pi/180;
%初始觀測(cè)值 初始觀測(cè)值設(shè)為0
Z=zeros(2,1);
%初始方差陣的設(shè)置
%初始速度誤差取0.1米每妙 初始失準(zhǔn)角均為1度
%加速度計(jì)的初始偏值均取1e-4*g 陀螺的常值漂移取0.1度每小時(shí)
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;
%＝＝＝＝＝＝＝＝＝＝＝＝＝＝＝初始系統(tǒng)噪聲誤差陣＝＝＝＝＝＝＝＝＝＝＝＝＝＝%
%加速度計(jì)的隨機(jī)偏差為0.5e-4*g 陀螺的隨機(jī)漂移為0.05度每小時(shí)
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;
%初始觀測(cè)噪聲誤差陣
R=zeros(2,2);
R(1,1)=0.1^2;
R(2,2)=0.1^2;
%==================狀態(tài)矩陣和系統(tǒng)噪聲陣的離散化====================%
T_discre=0.05; %離散化時(shí)間
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'); %創(chuàng)建文件，保存濾波結(jié)果
for i=1:number_data; %kalman濾波開(kāi)始
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';
%估計(jì)誤差的方差
ddeltav_e(i)=sqrt(P(1,1)); %速度估計(jì)誤差的方差 單位 m/s
ddeltav_n(i)=sqrt(P(2,2));
dfiee(i)=sqrt(P(3,3))*180*60*60/pi; %水平失準(zhǔn)角 單位角秒
dfien(i)=sqrt(P(4,4))*180*60*60/pi;
dfieu(i)=sqrt(P(5,5))*180*60/pi; %方位失準(zhǔn)角 單位角分
daccle_meter_x(i)=sqrt(P(6,6))*1e+6/g; %加速度計(jì)偏置 單位 ug
daccle_meter_y(i)=sqrt(P(7,7))*1e+6/g;
dgyro_x(i)=sqrt(P(8,8))*180*3600/pi; %陀螺漂移 單位度每小時(shí)
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; %保存三個(gè)姿態(tài)誤差角
% dfien(i)=b*60-(X(4,1))*180*60*60/pi; %水平誤差角的估計(jì)誤差單位 角秒
% dfieu(i)=c-(X(5,1))*180*60/pi; %方位誤差角的估計(jì)誤差單位 角分
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));
%保存濾波結(jié)果 即估計(jì)誤差的方差
end
%----------------------------------- 畫圖----------------------------------
t=0.05:0.05:60*5;
%估計(jì)誤差的方差曲線
figure(1)
plot(t,ddeltav_e)
xlabel('時(shí)間 /s')
ylabel('東向速度誤差 /（m/s）')
grid on
figure(2)
plot(t,ddeltav_n)
xlabel('時(shí)間 /s')
ylabel('北向速度誤差 /（m/s）')
grid on
figure(3)
plot(t,dfiee)
xlabel('時(shí)間 /s')
ylabel('東向失準(zhǔn)角 /（角秒）')
grid on
figure(4)
plot(t,dfien)
xlabel('時(shí)間 /s')
ylabel('北向失準(zhǔn)角 /（角秒）')
grid on
figure(5)
plot(t,dfieu)
xlabel('時(shí)間 /s')
ylabel('方位失準(zhǔn)角 /（角分）')
grid on
figure(6)
plot(t,daccle_meter_x)
xlabel('時(shí)間 /s')
ylabel('x向加速度計(jì)漂移 /（ug）')
grid on
figure(7)
plot(t,daccle_meter_y)
xlabel('時(shí)間 /s')
ylabel('y向加速度計(jì)漂移 /（ug）')
grid on
figure(8)
plot(t,dgyro_x)
xlabel('時(shí)間 /s')
ylabel('x向陀螺漂移 /（度每小時(shí)）')
grid on
figure(9)
plot(t,dgyro_y)
xlabel('時(shí)間 /s')
ylabel('y向陀螺漂移 /（度每小時(shí)）')
grid on
figure(10)
plot(t,dgyro_z)
xlabel('時(shí)間 /s')
ylabel('z向陀螺漂移 /（度每小時(shí)）')
grid on
?? 快捷鍵說(shuō)明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號(hào)
Ctrl + =
減小字號(hào)
Ctrl + -