本篇承接UWB那篇,专门介绍下卡尔曼滤波解决UWB无线时钟同步时的时间漂移的原理。
由于各基站的晶振或者别的硬件之间的个体差异,虽然有CCP,但计算出的同步时间还是不准。CCP每150ms发送一次,用本次收到CCP去同步完时间后,和150ms后再次收到CCP去同步完后时间比较并不完全一致,虽然差值只在皮秒级别,但对于光速来说还是不可接受的,会导致结果出现几米的误差。分析原因应该是150ms间两个晶振间出现了时钟漂移,虽然很小,但对结果还是有影响。而且观察这个漂移量,发现并没有什么规律,是个非线性的值。
这里就记录一下用卡尔曼滤波的方法跟踪这个时钟漂移。
卡尔曼滤波的理论
卡尔曼滤波应称为最优估计理论,这里的滤波和常规滤波是完全不同的含义。卡尔曼滤波是从与被提取信号有关的量测量中,通过算法估计出所需信号。
比如此项目中,时钟误差存在,时钟漂移率虽不是线性,但也跟前几次有一定关系。如果能通过前几次数据的误差估算出新到数据的误差,消除这个误差便能够提供系统的准确性。
卡尔曼滤波的应用
-
初始化:
N:设置卡尔曼滤波器追踪点数
r:设置估计变量个数,这里r=2 即S、V
s:从基站接收CCP时间戳 (输入量)
v:时钟漂移率 (输出估计量)
dt:主基站发送CCP的间隔 (输入量)
A:转移矩阵 2 * 2维 (包含输入量dt)
H:量测矩阵 1 * 2维
Qk:系统噪声矩阵 2 * 2维
Rk:量测噪声矩阵 1 * 1维
P0:均方误差矩阵初始值 2 * 2维
Y:状态矩阵,由k_s,k_v,k_a组成 (两个输出估计量) 2*2维
Y0:状态矩阵的初始值 2 * 2维Y0 = [0 1]'; (转置,2*1维) A = [1 dt;0 1]; (dt为主基站最近相邻两次CCP发送时间戳的间隔) H = [1 0]; Qk = [0 0;0 5*exp(-20)]; ( 5×10^(-20) ) Rk = 3*exp(-20); ( 3×10^(-20) ) P0 = [0 0;0 0];
-
方程:
Loop:
Y = A*Y;
P2 = A* P1*A’+Qk; (A’为A矩阵的转置矩阵)
Kk = P2* H’* inv(H* P2* H’+Rk); (inv表示逆矩阵)
Y = Y+Kk*(s-H*Y); (s为输入量,从基站接收的CCP时间戳)
P1 = (I-Kk*H)*P2; (I为二维单位阵)
End
输入量:每次从基站接收的CCP时间戳、主基站发送CCP的间隔
输出量:估计的从基站接收的CCP时间戳、时钟漂移率
- 过程:
每次从基站接收到CCP后,将接收时间戳作为s输入方程,将最近的主基站发送CCP的时间戳计算dt带入矩阵A输入。
执行整个循环得到新一轮估计值Y作为输出。Y包含两个值,一个是估计的从基站接收CCP时间戳θ,另一个是估计的时钟漂移率y。
当接收到信标信号时,记录当前时间戳t,从基站最近一次接收到的CCP时间戳Ts,对应的主基站CCP发送的时间戳Tm。则它同步到主基站的时间戳为:
[(t-Ts)/y]+Tm+Tms
(Tms为两基站间CCP飞行时间)
- 仿真结果:
另附一张当时手写的说明图:
仿真的程序和用到的MAT数据也会一并打包上传。
资源的连接:
https://download.csdn.net/download/weixin_44139651/12522973
github连接:
https://github.com/ypg666/syn