卡尔曼滤波解决UWB无线时钟同步时的时间漂移（含MATLAB仿真）

本篇承接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