基于卡尔曼滤波算法的主从时钟同步Matlab仿真

clear
clc;
N = 100;%采样次数
M = 10;%同步次数
offset = 10;%假设初始时钟偏差
drift = 20;%假设初始频率漂移
offset_seg = zeros(1,M);
drift_seg = zeros(1,M);
I = [1,0;0,1];
A = [1,2;0,1];
B = [-1,-2;0,-1];
%x_s = zeros(2,N);%保存暂态值
%%%%%%kalman filter%%%%%%%
off = zeros(1,N);   %估计的时钟偏差
dri = zeros(1,N);   %估计的频率漂移

%%初始值%%
x = [1;1]; 
offset_seg(1) = offset;
drift_seg(1) = drift;
u = [0;0];
Q = [var(randn(1,N)),0;0,var(randn(1,N))];%过程噪声协方差矩阵
P = Q;
R = [var(randn(1,N)),0;0,var(randn(1,N))];%观测噪声协方差矩阵
%%%%开始迭代%%%%
for k=2:M
z_off = 2^0.5 * randn(1,100)*0.5 + offset;%加过程噪声的时钟偏差输出
z_dri = 2^0.5 * randn(1,100)*0.5 + drift;%加过程噪声的频率漂移输出
z = [z_off;z_dri];
x = A* x+B*u;%x(n+1|n)=Ax_n+Bu_n