【信号去噪】基于Sage-Husa自适应卡尔曼滤波器实现海浪磁场噪声抑制及海浪磁场噪声的产生附matlab代码

1 内容介绍

一种海洋磁力仪海浪磁场噪声实时抑制方法,属于海洋地磁场探测领域.包括以下步骤:步骤一:启动海洋磁力仪,读取海洋地磁传感器的输出数据作为量测量;步骤二:建立系统状态方程和量测方程;步骤三:在t_(k1)时刻利用SageHusa自适应卡尔曼滤波器估计出t_k时刻的地磁总场值,并对系统噪声阵Q和量测噪声阵R进行更新和修正;步骤四:海洋磁力仪运行时间为M,若t_k=M,则保存数据,海洋磁力仪完成测量工作;若t_k

2 仿真代码

%%weaver模型模拟噪声  第二组磁异常数据

% sagehusa自适应卡尔曼与传统卡尔曼的对比（直观对比和残差对比）

clc;clear

Base_Value = 49600;   %基值

ab_single = ([49599.47

49597.37

49598.05

49597.9

49594.27

49584.91

49569.71

49541.43

49563.28

49663.24

49676.9

49673.31

49669.99

49666.46

49658.54

49647.31

49634.22

49627.6

49622.01

49618.89

49616.64

49613.81

49613.16

49613.77

]-Base_Value)./56 + Base_Value;

N = 500;   %噪声源点数

N1 = 30;   %噪声叠加次数

%%%%海浪噪声配置%%%%

z = 10;  %定深度

x = 10;  %定x

t = linspace(0,200,N);   % 采样率：f = 1/(200/500) = 2.5Hz

Noise = zeros(1,N);   

F = 0.488;   %地磁场强度

I = pi/4;    %磁偏角

Sita = 0;  

a = 100;    %cm  波浪高度  

g = 980;    %cm/s2        

sigama = 4*10^(-11);  %emu 电导率

w = 10*(0.314 + 0.314*rand(1,N1));  %w  0.314~0.628   角频率 

m =(w.^2)/g;  %波数

Bata = (4*pi*sigama*w)./(m.^2);  

C = cos(I)*cos(Sita);

S = sin(I);

A = a*m*F*(C*i + S);

%%%%%%%%%%%%% z>0时遭受叠加 %%%%%%%%%%%%%%%%%   

for j = 1:1:N1   

    Hx = (i*Bata(j)*A(j)/4)*(2*m(j)*z-1)*exp(i*w(j)*t-i*m(j)*x-m(j)*z);

    Hz = (Bata(j)*A(j)/4)*(2*m(j)*z+1)*exp(i*w(j)*t-i*m(j)*x-m(j)*z); 

    H11 = Hx*C + Hz*S;   %总磁场强度

    Noise = Noise + H11;    %叠加

end

%%%%%%%%%%%理想信号配置%%%%%%%%%%%%%%%

single = Base_Value*ones(1,N);

single(1,N/2:N/2+size(ab_single)-1) = ab_single;

%%%%%%%%%%%%信号加噪%%%%%%%%%%%%%%%%%%

sum_single = 6500000*Noise/56 + single; 

Z = sum_single;

% %%%%%%%%%%%%卡尔曼滤波器配置%%%%%%%%%%%%

% R = 10*std(sum_single)^2;    %测量噪声  

% Q = 0.025*R;    %系统噪声 

% p(1) = 10;      %增益初值

% kalman_out(1) = Base_Value;   %卡尔曼初值的选择要合理，大致落在数据区间内

% 

% for j = 2:1:N

% kalman_out1(j) = kalman_out(j-1);   %预测状态 

% p1(j) = p(j-1) + Q;     %预测估计协方差

% 

% kg(j) = p1(j) / (p1(j) + R);   % 最优卡尔曼增益

% kalman_out(j) = kalman_out1(j) + kg(j) * (sum_single(j) - kalman_out1(j));   %更新状态估计

% p(j) = (1 - kg(j)) * p1(j);     %更新协方差估计  

% end

%%%%%%%%%%%%sage-husa卡尔曼滤波器配置%%%%%%%%%%%%%

R1(1) = 0.7536;    %测量噪声  

Q1(1) = 0.0151;    %系统噪声  

P(1) = 1.5;       %增益初值

q(1) = 0;        %Q标准差初值

r(1) = 0;        %R标准差初值

v(1) = 0;

X(1) = Base_Value;   %卡尔曼初值的选择要合理，大致落在数据区间内

b = 0.95;        %遗忘因子

for j = 2:1:N

d = (1-b)/(1-b^(j+1));

% kalman_out1(j) = kalman_out(j-1) + q(j-1);   %预测状态 

X_(j) = X(j-1) ;   %预测状态  + q(j-1)

P_(j) = P(j-1) + Q1(j-1);     %预测估计协方差

r(j) = (1-d)*r(j-1) + d*(Z(j)-X_(j));

% v(j) = sum_single(j)-kalman_out1(j) - r(j);

v(j) = Z(j) - X_(j);  

% R(j) = (1-d)*R(j-1) + d*(v(j)*v(j) - p1(j));

R1(j) = (1-d)*R1(j-1);

K(j) = P_(j) / (P_(j) + R1(j-1));   % 最优卡尔曼增益

X(j) = X_(j) + K(j) * (Z(j) - X_(j));   %更新状态估计

P(j) = (1 - K(j)) * P_(j);     %更新协方差估计  

q(j) = (1-d)*q(j-1) + d*(X(j) - X(j-1));

% Q(j) = (1-d)*Q(j-1) + d*(kg(j)*kg(j)*v(j)*v(j) + p(j) - p(j-1) - 2*kg(j)*kg(j)*R(j) - 2*kg(j)*kg(j)*p1(j) + 2*p1(j)*kg(j));

Q1(j) = (1-d)*Q1(j-1);

end

% V_1 = kalman_out - single;  %Kalman输出残差

% V_2 = X - single;    %SageHusa输出残差

% figure(1);

% plot(t,sum_single,'g');

% hold on;

figure(2);

plot(t,R1,'g');

axis([0 200 -0.8 0.8]);

title('R');

figure(3);

plot(t,Q1,'r')

axis([0 200 -0.016 0.016]);

title('Q');

%%%%%%%%%%%%%%%%%残差数据输出到文件中%%%%%%%%%%%%%%%%%

% fid = fopen('SageHusa残差数据.txt','wt');

% fprintf(fid,'%g\n',abs(V_1(4:500)));

% fclose(fid);

% 

% fid1 = fopen('常规卡尔曼残差数据.txt','wt');

% fprintf(fid1,'%g\n',abs(V_2(4:500)));

% fclose(fid1);

% 

% fid2 = fopen('滤波前噪声.txt','wt');

% fprintf(fid2,'%g\n',6500000*Noise/56);

% fclose(fid2);

% 

% fid3 = fopen('卡尔曼滤波后噪声.txt','wt');

% fprintf(fid3,'%g\n',V_1);

% fclose(fid3);

% 

% fid4 = fopen('SageHusa滤波后噪声.txt','wt');

% fprintf(fid4,'%g\n',V_2);

% fclose(fid4);

% 

% fid4 = fopen('残差时间轴.txt','wt');

% fprintf(fid4,'%g\n',t(4:500));

% fclose(fid4);

%%%%%%%%%%%%%%%%%%%%%%% END %%%%%%%%%%%%%%%%%%%%%%%%%%%

3 运行结果

4 参考文献

[1]王福军, 丁小燕, 王前,等. 自适应强跟踪Sage-Husa卡尔曼滤波器载波环设计[J]. 电光与控制, 2019, 26(10):5.

[1]钱华明, 柏明明, 钱林琛,等. 一种海洋磁力仪海浪磁场噪声实时抑制方法:, CN107576989A[P]. 2018.

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。