目录
1.算法仿真效果
matlab2022a/Matlab2024b仿真结果如下：
2.MATLAB程序
........................................................
figure;
subplot(211);
plot(P1(1,:),'r');
hold on;
plot(P1(2,:),'b');
hold on;
plot(P1(3,:),'m');
xlabel('时间/s');
title('陀螺+磁强计');
legend('姿态估计值x','姿态估计值y','姿态估计值z');
subplot(212);
plot(E1(1,:),'r');
hold on;
plot(E1(2,:),'b');
hold on;
plot(E1(3,:),'m');
xlabel('时间/s');
legend('角速度修正值x','角速度修正值y','角速度修正值z');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;
subplot(211);
plot(P2(1,:),'r');
hold on;
plot(P2(2,:),'b');
hold on;
plot(P2(3,:),'m');
xlabel('时间/s');
title('星敏+陀螺');
legend('姿态估计值x','姿态估计值y','姿态估计值z');
subplot(212);
plot(E2(1,:),'r');
hold on;
plot(E2(2,:),'b');
hold on;
plot(E2(3,:),'m');
xlabel('时间/s');
legend('角速度修正值x','角速度修正值y','角速度修正值z');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;
subplot(211);
plot(P3(1,:),'r');
hold on;
plot(P3(2,:),'b');
hold on;
plot(P3(3,:),'m');
xlabel('时间/s');
title('太阳敏感器+陀螺');
legend('姿态估计值x','姿态估计值y','姿态估计值z');
subplot(212);
plot(E3(1,:),'r');
hold on;
plot(E3(2,:),'b');
hold on;
plot(E3(3,:),'m');
xlabel('时间/s');
legend('角速度修正值x','角速度修正值y','角速度修正值z');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;
subplot(211);
plot(P4(1,:),'r');
hold on;
plot(P4(2,:),'b');
hold on;
plot(P4(3,:),'m');
xlabel('时间/s');
title('磁强计+陀螺');
legend('姿态估计值x','姿态估计值y','姿态估计值z');
subplot(212);
plot(E4(1,:),'r');
hold on;
plot(E4(2,:),'b');
hold on;
plot(E4(3,:),'m');
xlabel('时间/s');
legend('角速度修正值x','角速度修正值y','角速度修正值z');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%ATSUKF进行联合滤波
disp('开始ATSUKF进行联合滤波');
Ps=[];
Es=[];
Qb = diag(0.0000002^2*ones(3,1));
Ts = 0.1;%根据原始数据的第一列得到采样时间间隔
for i=1:10000
if mod(i,50)==1
disp('ATSUKF联合滤波.....');
i
end
%输入四个传感器的姿态估计结果
Pa=P1(:,i);
Ea=E1(:,i);
Pb=P2(:,i);
Eb=E2(:,i);
Pc=P3(:,i);
Ec=E3(:,i);
Pd=P4(:,i);
Ed=E4(:,i);
%ATSUKF进行联合滤波，根据文献中公式1下面的Qx，Qb，R
Qx = diag([0.0015^2*[1;1;1];0.0015^2*[1;1;1]])*(pi/180)^2;%为系统噪声方差阵；
Qb = diag(0.00000015^2*ones(3,1)); %为故障噪声方差,这里可以解释为联合滤波过程中的融合误差
R = diag([0.008^2*[1;1;1];0.008^2*[1;1;1]])*(pi/180)^2; %量测噪声方差阵
%构造数据数组
Pak = [Ea,Ea,Ea];
Pbk = [Eb,Eb,Eb];
Pck = [Ec,Ec,Ec];
Pdk = [Ed,Ed,Ed];
Datba = [Pa,Pa,Pa,Pa,Pa,Pa];
Datbb = [Pb,Pb,Pb,Pb,Pb,Pb];
Datbc = [Pc,Pc,Pc,Pc,Pc,Pc];
Datbd = [Pd,Pd,Pd,Pd,Pd,Pd];
Pxk = [Pa,Pb,Pc,Pd,zeros(size(Pa)),zeros(size(Pa))];
if i == 1
ak1=0.25;
ak2=0.25;
ak3=0.25;
ak4=0.25;
else
ak1=mean2((Kxa))/[mean2((Kxa))+mean2((Kxb))+mean2((Kxc))+mean2((Kxd))];
ak2=mean2((Kxb))/[mean2((Kxa))+mean2((Kxb))+mean2((Kxc))+mean2((Kxd))];
ak3=mean2((Kxc))/[mean2((Kxa))+mean2((Kxb))+mean2((Kxc))+mean2((Kxd))];
ak4=mean2((Kxd))/[mean2((Kxa))+mean2((Kxb))+mean2((Kxc))+mean2((Kxd))];
end
%定义Fk
[Ak,Bk,Ck]= func_adp_ABC((ak1*Pa+ak2*Pb+ak3*Pc+ak4*Pd),Ts,I1,I2,I3);
Fa = 0.25*Bk;
Fs = [eye(3);zeros(3,3)];
r = Ak*[(ak1*Pa+ak2*Pb+ak3*Pc+ak4*Pd),(ak1*Pa+ak2*Pb+ak3*Pc+ak4*Pd),(ak1*Pa+ak2*Pb+ak3*Pc+ak4*Pd)] + Fa;
Pbi = ak1*Pak+ak2*Pbk + ak3*Pck+ak4*Pdk + Qb;
beta2 = r*Pbk/Pbi; %% k表示k|k，i表示k+1|k, s表示k+1|k+1；
Hi = Ck*beta2 + Fs;
%ATSUKF
L = 4;
lambda = 1e-6;
W1 = 2.5e-6;
W2 = 3;
W3 = 1.25;
W4 = 1.25;
%自适应S矩阵
[S,X] = func_adp_S((ak1*Pa+ak2*Pb+ak3*Pc+ak4*Pd),Pxk,lambda,L,r,Ea,Eb,beta2,Ts);
xi = X*[W1;W3*ones(12,1)];
X_var = X(:,2:13);
Z = zeros(6,13);
for i=1:13
Z(:,i)=S(:,i);
end
Zw = Z*[W1;W3*ones(12,1)];
Z1 = Z(:,2:13);
M = 6;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Datba =[Datba(:,2:M),Pa-Ck*xi];
SR = ((1/(M-1)*Datba*Datba')-(W2*(Z(:,1)-Zw)*(Z(:,1)-Zw)' + W4*(Z1-Zw*ones(1,12))*(Z1-Zw*ones(1,12))'))/R;
SR = diag(max(1,diag(SR)));
Pfm = W2*(Z(:,1)-Zw)*(Z(:,1)-Zw)' + W4*(Z1-Zw*ones(1,12))*(Z1-Zw*ones(1,12))' + SR*R;
Qar = Ck\(1/(M-1)*Datba*Datba'-Ck*(W2*(X(:,1)-xi)*(X(:,1)-xi)' + W4*(X_var-xi*ones(1,12))*(X_var-xi*ones(1,12))' + r*[Pak]*r' - beta2*Pbi*beta2')*Ck'-R)/(Qx*Ck');
Qar = diag(max(1,diag(Qar)));
Pfz = W2*(X(:,1)-xi)*(Z(:,1)-Zw)' + W4*(X_var-xi*ones(1,12))*(Z1-Zw*ones(1,12))';
Kxa = Pfm/Pfz;
Datbb =[Datbb(:,2:M),Pb-Ck*xi];
SR = ((1/(M-1)*Datbb*Datbb')-(W2*(Z(:,1)-Zw)*(Z(:,1)-Zw)' + W4*(Z1-Zw*ones(1,12))*(Z1-Zw*ones(1,12))'))/R;
SR = diag(max(1,diag(SR)));
Pfm = W2*(Z(:,1)-Zw)*(Z(:,1)-Zw)' + W4*(Z1-Zw*ones(1,12))*(Z1-Zw*ones(1,12))' + SR*R;
Qar = Ck\(1/(M-1)*Datbb*Datbb'-Ck*(W2*(X(:,1)-xi)*(X(:,1)-xi)' + W4*(X_var-xi*ones(1,12))*(X_var-xi*ones(1,12))' + r*[Pbk]*r' - beta2*Pbi*beta2')*Ck'-R)/(Qx*Ck');
Qar = diag(max(1,diag(Qar)));
Pfz = W2*(X(:,1)-xi)*(Z(:,1)-Zw)' + W4*(X_var-xi*ones(1,12))*(Z1-Zw*ones(1,12))';
Kxb = Pfm/Pfz;
Datbc =[Datbc(:,2:M),Pc-Ck*xi];
SR = ((1/(M-1)*Datbc*Datbc')-(W2*(Z(:,1)-Zw)*(Z(:,1)-Zw)' + W4*(Z1-Zw*ones(1,12))*(Z1-Zw*ones(1,12))'))/R;
SR = diag(max(1,diag(SR)));
Pfm = W2*(Z(:,1)-Zw)*(Z(:,1)-Zw)' + W4*(Z1-Zw*ones(1,12))*(Z1-Zw*ones(1,12))' + SR*R;
Qar = Ck\(1/(M-1)*Datbc*Datbc'-Ck*(W2*(X(:,1)-xi)*(X(:,1)-xi)' + W4*(X_var-xi*ones(1,12))*(X_var-xi*ones(1,12))' + r*[Pck]*r' - beta2*Pbi*beta2')*Ck'-R)/(Qx*Ck');
Qar = diag(max(1,diag(Qar)));
Pfz = W2*(X(:,1)-xi)*(Z(:,1)-Zw)' + W4*(X_var-xi*ones(1,12))*(Z1-Zw*ones(1,12))';
Kxc = Pfm/Pfz;
Datbd =[Datbd(:,2:M),Pc-Ck*xi];
SR = ((1/(M-1)*Datbd*Datbd')-(W2*(Z(:,1)-Zw)*(Z(:,1)-Zw)' + W4*(Z1-Zw*ones(1,12))*(Z1-Zw*ones(1,12))'))/R;
SR = diag(max(1,diag(SR)));
Pfm = W2*(Z(:,1)-Zw)*(Z(:,1)-Zw)' + W4*(Z1-Zw*ones(1,12))*(Z1-Zw*ones(1,12))' + SR*R;
Qar = Ck\(1/(M-1)*Datbd*Datbd'-Ck*(W2*(X(:,1)-xi)*(X(:,1)-xi)' + W4*(X_var-xi*ones(1,12))*(X_var-xi*ones(1,12))' + r*[Pdk]*r' - beta2*Pbi*beta2')*Ck'-R)/(Qx*Ck');
Qar = diag(max(1,diag(Qar)));
Pfz = W2*(X(:,1)-xi)*(Z(:,1)-Zw)' + W4*(X_var-xi*ones(1,12))*(Z1-Zw*ones(1,12))';
Kxd = Pfm/Pfz;
alphas = 0.125;
%联合滤波输出
XFilter_toegther = xi + alphas*[Kxa*(Pa-Ck*xi)+Kxb*(Pb-Ck*xi)+Kxc*(Pc-Ck*xi)+Kxd*(Pd-Ck*xi)];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Datba = [Datba(:,2:M),Pa-Ck*xi-Hi*Ea];
Qbr = (Hi\(1/(M-1)*Datba*Datba'-(W2*(Z(:,1)-Zw)*(Z(:,1)-Zw)' + W4*(Z1-Zw*ones(1,12))*(Z1-Zw*ones(1,12))') -R)/Hi'-Pak)/(Qb);
Qbr = diag(max(1,diag(Qbr)));
Qbia = Qbr*Qb;
Datbb = [Datbb(:,2:M),Pb-Ck*xi-Hi*Eb];
Qbr = (Hi\(1/(M-1)*Datbb*Datbb'-(W2*(Z(:,1)-Zw)*(Z(:,1)-Zw)' + W4*(Z1-Zw*ones(1,12))*(Z1-Zw*ones(1,12))') -R)/Hi'-Pbk)/(Qb);
Qbr = diag(max(1,diag(Qbr)));
Qbib = Qbr*Qb;
Datbc = [Datbc(:,2:M),Pc-Ck*xi-Hi*Ec];
Qbr = (Hi\(1/(M-1)*Datbc*Datbc'-(W2*(Z(:,1)-Zw)*(Z(:,1)-Zw)' + W4*(Z1-Zw*ones(1,12))*(Z1-Zw*ones(1,12))') -R)/Hi'-Pck)/(Qb);
Qbr = diag(max(1,diag(Qbr)));
Qbic = Qbr*Qb;
Datbd = [Datbd(:,2:M),Pd-Ck*xi-Hi*Ed];
Qbr = (Hi\(1/(M-1)*Datbd*Datbd'-(W2*(Z(:,1)-Zw)*(Z(:,1)-Zw)' + W4*(Z1-Zw*ones(1,12))*(Z1-Zw*ones(1,12))') -R)/Hi'-Pdk)/(Qb);
Qbr = diag(max(1,diag(Qbr)));
Qbid = Qbr*Qb;
Pbi = [ak1*Pak+ak2*Pbk+ak3*Pck+ak4*Pdk+ak1*Qbia+ak2*Qbib+ak3*Qbic+ak4*Qbid];
Kb = Pbi*Hi'/(Pfm + Hi*Pbi*Hi');
WFilter_toegther =(eye(3)-Kb*Hi)*Pbi;
Ps = [Ps,XFilter_toegther(:,1)];
Es = [Es,WFilter_toegther(:,1)];
end
Ps=Ps-Ps(:,1);
figure;
subplot(211);
plot(Ps(1,:),'r');
hold on;
plot(Ps(2,:),'b');
hold on;
plot(Ps(3,:),'m');
xlabel('时间/s');
title('ATSUFK联合滤波');
legend('姿态估计值x','姿态估计值y','姿态估计值z');
subplot(212);
plot(Es(1,:),'r');
hold on;
plot(Es(2,:),'b');
hold on;
plot(Es(3,:),'m');
xlabel('时间/s');
legend('角速度修正值x','角速度修正值y','角速度修正值z');
17
3.算法概述
卫星姿态控制是卫星实现其各项任务的关键技术之一，准确的姿态估计对于卫星的稳定运行、有效载荷的正常工作以及轨道控制等方面都具有至关重要的意义。在卫星姿态估计中，由于测量数据存在噪声、系统模型存在不确定性等因素，需要采用合适的滤波算法来提高姿态估计的精度和可靠性。自适应无迹容积卡尔曼滤波（Adaptive Truncated Sigma - Point Unscented Kalman Filter，ATSUKF）算法是一种在非线性系统状态估计中表现出色的算法，能够有效处理卫星姿控系统中的非线性问题和不确定性.
在实际应用中，系统的噪声统计特性可能会发生变化，传统的 UKF 算法假设噪声协方差矩阵是固定的，这可能会导致滤波性能下降。ATSUKF 算法引入了自适应机制，通过实时估计噪声协方差矩阵来提高滤波的鲁棒性。
ATSUKF 算法步骤如下
基于 ATSUKF 算法的卫星姿控估计方法结合了自适应机制和截断 Sigma 点策略，能够有效处理卫星姿控系统中的非线性问题和噪声统计特性变化的情况，同时减少计算量，提高滤波的精度和鲁棒性。通过合理选择状态向量、系统模型和测量模型，并按照 ATSUKF 算法的步骤进行迭代计算，可以实现对卫星姿态的准确估计，为卫星的姿态控制提供可靠的依据。
4.部分参考文献
[1]陈雪芹,孙瑞,吴凡,等.基于ATSUKF算法的卫星姿控系统故障估计[J].航空学报, 2019, 40(5):10.DOI:10.7527/S1000-6893.2018.22551.
5.程序内容，运行方法和源码获取
5.1 作品内容
matlab程序，参考文献，说明文档
5.2 运行方法
1.在matlab的左侧的当前文件夹窗口；
2.运行ATSUKF_tops文件；
5.3 源码获取
step1.打开博客主页的左侧推广栏查看，或扫博客文章底部信息
step2.然后用电脑打开网页链接，输入文章标题搜索
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
