一、參考資料
1、基於強跟蹤濾波器的在線故障診斷方法
2、線性連續系統的離散化
二、理論推導
畫重點:當考慮系統受到載荷激勵時,微分方程右邊爲載荷激勵;當考慮系統受到位移和速度的激勵時,右邊爲位移速度激勵以剛度k和阻尼c爲係數的線性合作。這是兩個不同的動力學系統。如果要卡爾曼濾波監測k和c,則需要擴展k和c兩個狀態變量,這時只能用第二種系統模型,否則卡爾曼濾波在狀態預測時,不準確的k和c的估計值會影響到載荷激勵,與真實的載荷差別很大,使估計誤差的方差不僅不減小,反而會增大,致使算法無法收斂。模型錯誤是卡爾曼濾波不收斂的重要原因。
三、線性系統離散及驗證
四、強跟蹤卡爾曼濾波故障診斷
假設系統初始剛度爲7.55/(pi/180),0.3秒後系統發生故障,剛度變成450/(pi/180),剛度初始估計值爲0.5*7.55/(pi/180),看卡爾曼濾波可否快速跟蹤剛度的變化從而進行故障判斷。
隔振系統仿真代碼
%kalman_sim.m
%Plant
function [u]=kalman_sim(u1,u2,u3,u4,u5)
%u1:輸入飛輪位移
s_flywheel=u1;
%u2:輸入飛輪轉速
v_flywheel=u2;
%u3:系統噪聲W
W=u3;
%u4:測量噪聲V
V=u4;
%u5:時間t
t=u5;
%s一軸轉角 v一軸轉速 m一軸當量質量 k離合器剛度 c離合器阻尼
%Ts採樣時間(由輸入信號採樣頻率確定)
%F_s_flywheel飛輪轉角 F_v_flywheel飛輪轉速
persistent s v m k c Ts
if t==0
fs=1792.1;
Ts=1/fs;
s=0.0;
v=0.0;
m=0.3511;%kgm^2
k=7.55/(pi/180);%Nm/rad
wn=sqrt(k/m);
s_=0.25;
%wd=sqrt(1-s_^2)*wn;
c=2*s_*wn*m;
z=v+V;
end
if t>0.3
k=450/(pi/180);%假設5秒後剛度出現突變爲450
end
if t>0
F_input=c*v_flywheel+k*s_flywheel;
s=s+Ts*v;
v=-k/m*Ts*s+(1-c/m*Ts)*v+Ts/m*F_input+Ts*W;
z=v+V;
end
u(1)=v;
u(2)=k;
u(3)=c;
u(4)=z;
強跟蹤卡爾曼濾波代碼
%kalman_k_c.m
%Plant
function [u]=kalman_sim(u1,u2,u3,u4)
%u1:輸入飛輪位移
s_flywheel=u1;
%u2:輸入飛輪轉速
v_flywheel=u2;
%u3:系統噪聲W
z=u3;
%u4:時間t
t=u4;
%persistent v m i
persistent x Ts m Q R P H Jf V0
if t==0
fs=1792.1;
Ts=1/fs;
m=0.3511;%kgm^2
k=7.55/(pi/180);%Nm/rad
wn=sqrt(k/m);
s_=0.25;
%wd=sqrt(1-s_^2)*wn;
c=2*s_*wn*m;
%系統方程:
H=[0,1,0,0];
%Covariances of w:
Q=diag([0,Ts*0.1*Ts',0,0]);
%Covariances of v:
R=[0.1];
%初始估計值:
x=[0;0;k/2;c];
%初始估計誤差協方差:
P=diag([0,Ts*0.1*Ts',40000,0.000000]);
u=x(2:4);%開始時刻不進行卡爾曼濾波,直接將初始值輸出,2020
end
%%卡爾曼濾波::
if t>0
%先驗,Time update:
%根據系統狀態方程計算下一狀態預測值:
%x=A*x+B*u1;
Jf=eye(4);
Jf(1,2)=Ts;
Jf(2,1)=-x(3)/m*Ts;
Jf(2,2)=1-x(4)/m*Ts;
%*********very important**********
%因爲激勵爲飛輪的轉角與轉速而不是飛輪對離合器的激勵轉矩,改動如下:
%Jf(2,3)=-x(1)/m*Ts;
Jf(2,3)=(s_flywheel-x(1))/m*Ts;
%Jf(2,4)=-x(2)/m*Ts;
Jf(2,4)=(v_flywheel-x(2))/m*Ts;
%**********************************
s=x(1);
v=x(2);
k=x(3);
c=x(4);
F_input=c*v_flywheel+k*s_flywheel;
s=s+Ts*v;
v=-k/m*Ts*s+(1-c/m*Ts)*v+Ts/m*F_input;
x(1)=s;
x(2)=v;
x(3)=k;
x(4)=c;
%************強跟蹤濾波器STF核心代碼《::*********************************
%**************************************
% 可調節參數彙總
%**************************************
rho=0.95; % 殘差方差陣的遺忘因子初始值 (0.95<rho<0.995)
beta=1; % 量測噪聲的衰減因子選定值 (beta>=1)
%beta_upmax=8; % 衰減因子選定極限
%***************************************
F=Jf;
r0=z-H*x;
if t<2*Ts
V0=r0*r0';
else
V0=(rho*V0+r0*r0')/(1+rho);
end
N=V0-beta*R-H*Q*H';
M=H*F*P*F'*H';
eta=trace(N)/trace(M);
if eta>1
lamda=eta;
else
lamda=1;
end
%預測誤差協方差:
P=lamda*Jf*P*Jf'+Q;
%!!!漸消因子lamda=1即爲普通卡爾曼濾波!!!
%************STF核心代碼》************************************************
%後驗,Measurement update:
%根據估計誤差協方差和測量噪聲協方差計算卡爾曼增益:
Kk=P*H'/(H*P*H'+R);
%計算最優估計值:
%x=A*x+Mn*(yv-C*A*x);
x=x+Kk*(z-H*x);
%計算最優估計值和真實值之間的誤差協方差矩陣,爲下次遞推做準備:
P=(eye(4)-Kk*H)*P;
%ye=C*x; %Filtered value
u=x(2:4); %Filtered signal
errcov=H*P*H'; %Covariance of estimation error
end
%}