Kalman滤波锁相及Matlab仿真

采用卡尔曼滤波算法锁相信号为正弦波，考虑幅值、谐波、频率变化的影响，给出Matlab仿真的S-Function函数Kalman Filter原理

function [sys,x0,str,ts] = KalmanFilter(t,x,u,flag)

% x(n+1) = Ax(n) + Bu(n)% y(n) = Cx(n) + Du(n) % See sfuntmpl.m for a general S-function template.f14000= 50;Ts = 0.0001;A = [cos(2*pi*f1*Ts) -sin(2*pi*f1*Ts) sin(2*pi*f1*Ts) cos(2*pi*f1*Ts)];H = [1 0];R = 1;Q = [0.001 0;0 0.001]; %2*2%Q = [0.0110; 0 0.011]*A*A;%R = 20*A*A;X0= [0;-2];P0 = [0;0;0;0]; %4*1 -> 2*2I = [1 0;0 1];switch flag, case 0, [sys,x0,str,ts] = mdlInitializeSizes(t,X0,P0); case 2, sys = mdlUpdate(t,x,u,A,Q,R,H,I); case 3, sys = mdlOutputs(t,x); case 9, sys = []; % do nothing otherwiseDAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));endfunction [sys,x0,str,ts] = mdlInitializeSizes(t,X0,P0)sizes = simsizes;sizes.NumContStates = 0;sizes.NumDiscStates = 6;sizes.NumOutputs = 4; %sinTheta cosTheta cosTheta magnitudesizes.NumInputs= 1; %Ua_measured = Z <- usizes.DirFeedthrough = 1;sizes.NumSampleTimes = 1;sys = simsizes(sizes);x0 = [X0;P0]; %collumstr = [];ts = [0.0001 0];function sys = mdlUpdate(t,x,u,A,Q,R,H,I)Z = u; %input Ua_measuredX1 = [x(1);x(2)]; %state X 2*1P1 = [x(3) x(4);x(5)x(6)]; %state P 2*2Xmid = A*X1; %mid X 2*1Pmid = A*P1*A.'+Q; %mid P 2*2K = Pmid*H.'/(H*Pmid*H.'+R); %2*1/number(1 dim)X2 = Xmid+K*(Z-H*Xmid); %update X 2*1P2 = (I-K*H)*Pmid; %update P 2*2sys = [X2(1);X2(2);P2(1,1);P2(1,2);P2(2,1);P2(2,2)];function sys = mdlOutputs(t,x,u)X= [x(1);x(2)];sinTheta = X(1,1);cosTheta = X(2,1);theta = atan2(sinTheta,-cosTheta);magnitude = hypot(sinTheta,cosTheta);sinTheta = sinTheta/magnitude;cosTheta = cosTheta/magnitude;sys = [sinTheta;cosTheta;theta;magnitude];