📄 init_kalman.m
字号:
function FilterPM=Init_Kalman(Algorithm_PM,G_drift,A_bias)
switch Algorithm_PM.app
case 'Velocity_Attitude_Euler'
switch Algorithm_PM.error
case 'Consider'
X=zeros(13,1);
P = eye(13);
Q = zeros(13);i=1;
R = zeros(5);j=1;
P(1,1) = power(0.1,2);
P(2,2) = power(0.1,2);
P(3,3) = power(1*pi/180,2);
P(4,4) = power(1*pi/180,2);
P(5,5) = power(1*pi/180,2);
P(6,6) = power(1*pi/180,2);
P(7,7) = power(1*pi/180,2);
P(8,8) = power(1*pi/180,2);
P(9,9) = power(A_bias(1),2);
P(10,10) = power(A_bias(2),2);
P(11,11) = power(G_drift(1),2);
P(12,12) = power(G_drift(2),2);
P(13,13) = power(G_drift(3),2);
Q(1,1) = power(i*0.1*A_bias(1),2);
Q(2,2) = power(i*0.1*A_bias(2),2);
Q(3,3) = power(i*0.1*G_drift(1),2);
Q(4,4) = power(i*0.1*G_drift(2),2);
Q(5,5) = power(i*0.1*G_drift(3),2);
Q(6,6) = power(i*0.1*G_drift(1),2);
Q(7,7) = power(i*0.1*G_drift(2),2);
Q(8,8) = power(i*0.1*G_drift(3),2);
Q(9,9) = power(i*0.1*A_bias(1),2);
Q(10,10) = power(i*0.1*A_bias(2),2);
Q(11,11) = power(i*0.1*G_drift(1),2);
Q(12,12) = power(i*0.1*G_drift(2),2);
Q(13,13) = power(i*0.1*G_drift(3),2);
R(1,1) = power(j*0.001,2);
R(2,2) = power(j*0.001,2);
R(3,3) = power(j*1/60*pi/180,2);
R(4,4) = power(j*1/60*pi/180,2);
R(5,5) = power(j*1/60*pi/180,2);
case 'LevelArm_Consider'
X=zeros(16,1);
P = eye(16);
Q = zeros(16);i=1;
R = zeros(5);j=1;
P(1,1) = power(0.1,2);
P(2,2) = power(0.1,2);
P(3,3) = power(1*pi/180,2);
P(4,4) = power(1*pi/180,2);
P(5,5) = power(1*pi/180,2);
P(6,6) = power(1*pi/180,2);
P(7,7) = power(1*pi/180,2);
P(8,8) = power(1*pi/180,2);
P(9,9) = power(A_bias(1),2);
P(10,10) = power(A_bias(2),2);
P(11,11) = power(G_drift(1),2);
P(12,12) = power(G_drift(2),2);
P(13,13) = power(G_drift(3),2);
P(14,14) = power(0.1,2);
P(15,15) = power(0.1,2);
P(16,16) = power(0.1,2);
Q(1,1) = power(i*0.1*A_bias(1),2);
Q(2,2) = power(i*0.1*A_bias(2),2);
Q(3,3) = power(i*0.1*G_drift(1),2);
Q(4,4) = power(i*0.1*G_drift(2),2);
Q(5,5) = power(i*0.1*G_drift(3),2);
Q(6,6) = power(i*0.1*G_drift(1),2);
Q(7,7) = power(i*0.1*G_drift(2),2);
Q(8,8) = power(i*0.1*G_drift(3),2);
Q(9,9) = power(i*0.1*G_drift(1),2);
Q(10,10) = power(i*0.1*G_drift(2),2);
Q(11,11) = power(i*0.1*G_drift(3),2);
Q(12,12) = power(i*0.1*A_bias(1),2);
Q(13,13) = power(i*0.1*A_bias(2),2);
Q(14,14) = power(0.001,2);
Q(15,15) = power(0.001,2);
Q(16,16) = power(0.001,2);
R(1,1) = power(j*0.001,2);
R(2,2) = power(j*0.001,2);
R(3,3) = power(j*1/60*pi/180,2);
R(4,4) = power(j*1/60*pi/180,2);
R(5,5) = power(j*1/60*pi/180,2);
end
case 'Velocity_Attitude_Quaternion'
switch Algorithm_PM.error
case 'Consider'
Q_m1=dcm_qua(eulr_dcm(0.1*G_drift));
Q_m2=dcm_qua(eulr_dcm(G_drift));
X=zeros(15,1);X(3)=1;X(7)=1;
P = eye(15);
Q = zeros(15);i=1;
R = zeros(6);j=1;
P(1,1) = power(0.1,2);
P(2,2) = power(0.1,2);
switch Algorithm_PM.app_detail
case 'algorithm1'
m=0.5;
P(3,3) = power(m*0.05,2);
P(4,4) = power(m*0.05,2);
P(5,5) = power(m*0.05,2);
P(6,6) = power(m*0.05,2);
P(7,7) = power(m*0.05,2);
P(8,8) = power(m*0.05,2);
P(9,9) = power(m*0.05,2);
P(10,10) = power(m*0.05,2);
case 'algorithm2'
m=1; X(3)=0;X(7)=0;
P(3,3) = power(m*0.05,2);
P(4,4) = power(m*0.05,2);
P(5,5) = power(m*0.05,2);
P(6,6) = power(m*0.05,2);
P(7,7) = power(m*0.01,2);
P(8,8) = power(m*0.01,2);
P(9,9) = power(m*0.01,2);
P(10,10) = power(m*0.01,2);
end
P(11,11) = power(A_bias(2),2);
P(12,12) = power(A_bias(2),2);
P(13,13) = power(G_drift(1),2);
P(14,14) = power(G_drift(2),2);
P(15,15) = power(G_drift(3),2);
Q(1,1) = power(i*0.1*A_bias(1),2);
Q(2,2) = power(i*0.1*A_bias(2),2);
Q(3,3) = power(i*0.1*G_drift(1),2);
Q(4,4) = power(i*0.1*G_drift(1),2);
Q(5,5) = power(i*0.1*G_drift(2),2);
Q(6,6) = power(i*0.1*G_drift(3),2);
Q(7,7) = power(i*0.1*G_drift(1),2);
Q(8,8) = power(i*0.1*G_drift(1),2);
Q(9,9) = power(i*0.1*G_drift(2),2);
Q(10,10) = power(i*0.1*G_drift(3),2);
Q(11,11) = power(i*0.1*A_bias(1),2);
Q(12,12) = power(i*0.1*A_bias(2),2);
Q(13,13) = power(i*0.1*G_drift(1),2);
Q(14,14) = power(i*0.1*G_drift(2),2);
Q(15,15) = power(i*0.1*G_drift(3),2);
R(1,1) = power(j*0.001,2);
R(2,2) = power(j*0.001,2);
R(3,3) = power(i*60*G_drift(1),2);
R(4,4) = power(i*60*G_drift(1),2);
R(5,5) = power(i*60*G_drift(2),2);
R(6,6) = power(i*60*G_drift(3),2);
case 'LevelArm_Consider'
Q_m1=dcm_qua(eulr_dcm(0.1*G_drift));
Q_m2=dcm_qua(eulr_dcm(G_drift));
X=zeros(18,1);X(3)=1;X(7)=1;
P = eye(18);
Q = zeros(18);i=1;
R = zeros(6);j=1;
P(1,1) = power(0.1,2);
P(2,2) = power(0.1,2);
switch Algorithm_PM.app_detail
case 'algorithm1'
P(3,3) = power(0.05,2);
P(4,4) = power(0.05,2);
P(5,5) = power(0.05,2);
P(6,6) = power(0.05,2);
P(7,7) = power(0.05,2);
P(8,8) = power(0.05,2);
P(9,9) = power(0.05,2);
P(10,10) = power(0.05,2);
case 'algorithm2'
X(3)=0;X(7)=0;
P(3,3) = power(0.01,2);
P(4,4) = power(0.01,2);
P(5,5) = power(0.01,2);
P(6,6) = power(0.01,2);
P(7,7) = power(0.01,2);
P(8,8) = power(0.01,2);
P(9,9) = power(0.01,2);
P(10,10) = power(0.01,2);
end
P(11,11) = power(A_bias(2),2);
P(12,12) = power(A_bias(2),2);
P(13,13) = power(G_drift(1),2);
P(14,14) = power(G_drift(2),2);
P(15,15) = power(G_drift(3),2);
P(16,16) = power(0.1,2);
P(17,17) = power(0.1,2);
P(18,18) = power(0.1,2);
Q(1,1) = power(i*0.1*A_bias(1),2);
Q(2,2) = power(i*0.1*A_bias(2),2);
Q(3,3) = power(i*0.1*G_drift(1),2);
Q(4,4) = power(i*0.1*G_drift(1),2);
Q(5,5) = power(i*0.1*G_drift(2),2);
Q(6,6) = power(i*0.1*G_drift(3),2);
Q(7,7) = power(i*0.1*G_drift(1),2);
Q(8,8) = power(i*0.1*G_drift(1),2);
Q(9,9) = power(i*0.1*G_drift(2),2);
Q(10,10) = power(i*0.1*G_drift(3),2);
Q(11,11) = power(i*0.1*A_bias(1),2);
Q(12,12) = power(i*0.1*A_bias(2),2);
Q(13,13) = power(i*0.1*G_drift(1),2);
Q(14,14) = power(i*0.1*G_drift(2),2);
Q(15,15) = power(i*0.1*G_drift(3),2);
Q(16,16) = power(0.001,2);
Q(17,17) = power(0.001,2);
Q(18,18) = power(0.001,2);
R(1,1) = power(j*0.001,2);
R(2,2) = power(j*0.001,2);
R(3,3) = power(i*60*G_drift(1),2);
R(4,4) = power(i*60*G_drift(1),2);
R(5,5) = power(i*60*G_drift(2),2);
R(6,6) = power(i*60*G_drift(3),2);
end
end
FilterPM.X=X;
FilterPM.P=P;
FilterPM.P0=P;
FilterPM.re=0*P;
FilterPM.Q=Q;
FilterPM.R=R;
FilterPM.G_drift=G_drift;
FilterPM.A_bias=A_bias;
FilterPM.pwcs.eigenvalue=zeros(size(X));⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -