扩展卡尔曼滤波器(EKF)进行信号处理及信号参数估计

% 扩展卡尔曼滤波器估计单相电压幅值、相位、频率参数（含直流）function test2_EKF

close all;

clc;

tic; %计时

%模型：y=A0+A1*cos(omega*t+phy1)

%离散化：y(k)=A0(k)+A1(k)*cos(omega(k)*k*Ts+phy1(k))

%状态变量：x1(k)=A0(k),x2(k)=omega(k),x3(k)=A1(k)*cos(omega(k)*k*Ts+phy1(k) ),x4(k)=A1(k)*sin(omega(k)*k*Ts+phy1(k))

%下一时刻状态变量为(假设状态不突变)：A0(k+1)=A0(k),A1(k+1)=A1(k),omega(k+1)=omega(k),phy1(k+1)=phy1 (k);

%则对应状态为：x1(k+1)=x1(k),x2(k+1)=x2(k),x3(k+1)=x3(k)*cos(x2(k)*Ts)-

x4(k)*sin(x(2)*Ts),x4(k+1)=x3(k)*sin(x2(k)*Ts)+x4(k)*cos(x(2)*Ts);

%状态空间描述：X(k+1)=f(X(k))+W(k);y(k)=H*X(k)+v(k)

%f(X(k))=[x1(k);x2(k);x3(k)*cos(x2(k)*Ts)-

x4(k)*sin(x(2)*Ts);x3(k)*sin(x2(k)*Ts)+x4(k)*cos(x(2)*Ts)]

%偏导(只求了三个)：f`(X(k))=[1,0,0;0,1,0;0,-x3(k)*Ts*sin(x2(k)*Ts)-x4(k)*Ts*cos(x2(k)*Ts),cos(x2(k)*Ts);0,x3(k)*Ts*cos(x2(k)*Ts)-

x4(k)*Ts*sin(x2(k)*Ts),sin(x2(k)*Ts)]

N=1000;

t=linspace(0,1,N);

y=2+0.5*cos(2*pi*100*t+pi/3);

y1=y+0.05*randn(size(y));

% p1=1*exp(-4*log(2)*(t-0.5).^2/0.005^2);

% y1=y1+p1;

% y1=y;

Ts=diff(t(1:2));

% plot(t,y)

% 状态空间描述：X(k+1)=f(X(k))+W(k);y(k)=H*X(k)+v(k);

X=zeros(4,N);

% X1=X;

X(:,1)=[0,199*pi,0,0];

Q=1e-7*eye(4);

R=1;

P=1e4*eye(4);

H=[1,0,1,0];

for j=2:N

X1=[X(1,j-1);X(2,j-1);X(3,j-1)*cos(X(2,j-1)*Ts)-X(4,j-1)*sin(X(2,j-1)*Ts);X(3,j-1)*sin(X(2,j-1)*Ts)+X(4,j-1)*cos(X(2,j-1)*Ts)];

F=[1,0,0,0

0,1,0,0

0,-X(3,j-1)*Ts*sin(X(2,j-1)*Ts)-X(4,j-1)*Ts*cos(X(2,j-1)*Ts),cos(X(2,j-1)*Ts),-sin(X(2,j-1)*Ts)

0,X(3,j-1)*Ts*cos(X(2,j-1)*Ts)-X(4,j-1)*Ts*sin(X(2,j-1)*Ts),sin(X(2,j-1)*Ts),cos(X(2,j-1)*Ts)];

P1=F*P*F'+Q;

K=P1*H'/(H*P1*H'+R);

X(:,j)=X1+K*(y1(j)-H*X1);

P=(eye(4)-K*H)*P1;

end

y2=H*X;

toc; %结束计时

subplot(2,3,1)

plot(t,y1)

hold on

plot(t,y2,'-',t,y,'--')

hold off

subplot(2,3,2)

plot(t,X(1,:)) %直流偏移

subplot(2,3,3)

plot(t,X(2,:)/2/pi) %频率

% ylim([5,15])

subplot(2,3,4)

% plot(t,y1-mean(y1)-y2)

plot(t,sqrt(X(3,:).^2+X(4,:).^2)) %幅值subplot(2,3,5)

plot(t,atan(X(4,:)./X(3,:))) %相位subplot(2,3,6)

plot(t,y1-y2) %误差