[发明专利]一种基于CKF、chan解算和Savitzky-Golay平滑滤波的位置解算方法

权利要求书

1.一种基于CKF、chan解算和Savitzky-Golay平滑滤波的位置解算方法，其特征在于具体操作步骤按照如下方式实现：

S1、使用TDOA测量值误差模型(1)：

其中，r_k表示当前标签距离基站的距离，r_k-1表示上一时刻标签距离基站的距离，T表示测量间隔，v为标签运动速度，a为标签运动加速度，Δr_k标签到主基站与从基站的距离差，Δr_k主，Δr_k从分别表示标签到主基站和从基站距离，Δt_ij为信号到达基站的实际时间差，为理想到达时间差，μ为测量传感器误差和环境误差，符合的高斯白噪声；

S2、CKF状态方程和测量方程建模：

将各个从基站到主基站的达到时间差和运动速度差作为系统状态向量X，且X表示为

X＝[Δr₀ Δr₁ Δr₂ Δv₀ Δv₁ Δv₂]^T (2)

其中Δr₀,Δr₁,Δr₂表示定位标签到定位从基站与主基站的距离差，Δv₀,Δv₁,Δv₂表示标签相对从基站和主基站的移动速度差，状态方程如下，

X_k＝FX_k-1+ΓW_k-1 (3)

其中，F为状态转移矩阵，表示为，

式中，T表示测量时间间隔，将标签运动加速度建模为系统噪声W_k-1，系数矩阵Γ为，

观测方程建模为，

Z_k＝HX_k+V_k (6)

式中，Z_k为观测量，表达式为

Z＝[Δr_m0 Δr_m1 Δr_m2 Δv_m0 Δv_m1 Δv_m2]^T (7)

H为，

V_k为测量噪声，包含测量传感器误差和环境误差，符合N(0，R)分布；

S3、按照如下步骤实现CKF流程：

a)时间更新

设定k-1时刻的状态后延概率密度已知，通过Cholesky分解误差协方差矩阵选择容积点，

式中，计算系统方程传递后的容积点，

估计k时刻状态的预测值，

b)量测更新

对P_k/k-1做Cholesky分解，

计算容积点，

通过量测方程传递容积点，

Z_i,k/k-1＝Hχ_i,k/k-1+V_k (14)

估计k时刻量测的预测值，

估计k时刻量测协方差阵和互相关协方差阵，

估计k时刻滤波增益，

K_k＝P_xz,k/k-1(P_z,k/k-1)^-1

(17)

求k时刻状态值，

求k时刻状态误差协方差估计，

通过以上流程，再对滤波后数据时间同步，即可完成对TDOA测量值进行处理，进而对标签位置进行解算；

S4、使用chan算法进行位置解算：

接收定位基站发送的TDOA测量信息后，进行CKF处理，处理后的值作为chan算法的输入量，其中，chan算法其详细处理流程如下：

设定待确定的人员位置(x，y)，定位基站的坐标(x_i，y_i)，i＝1,2…，设定(x1,y1)为主基站，其余为从基站；

通过双曲线模型获得目标的位置估计，则有：

r_i²＝(r_i,1+r₁)²

(21)

其中，r_i为标签到基站的距离，i＝1,2…，r_i,1为从基站到主基站的距离；

对式(20)进行移项处理后平方可得：

r_i²＝(x_i-x)²+(y_i-y)²＝K_i-2x_ix-2y_iy+x²+y² (22)

式中，i＝1,2…，

将式(21)和式(22)联立：

令i＝1，式(20)得到：

r₁²＝K₁-2x₁x-2y₁y+x²+y²

(24)

式(23)减式(24)得到：

设定可使用的基站为N个，结合式(25)，并且假设x、y和r₁三者之间线性无关；令z_p＝[xy]^T则有：

h＝G_az_a

(26)

式中，

以z_a为未知量，使用加权最小二乘法求解该非线性方程，即：

其中，误差矢量Ψ可近似为具有协方差矩阵的高斯随机矢量；

表达式如下，

ψ＝cBn+0.5c²n⊙n

Ψ＝E[ψψ^T]＝c²BQB

(29)式中，c为电磁波在空气中传播速度，为TDOA测量值的协方差矩阵，n为Hahn和Tretter噪声向量；

再次使用加权最小二乘法进行估计，式(26)可化简为，

ψ'＝h'＝G_a'z_a' (30)

式中，

这里Ψ′为z_a′的误差矢量。Ψ′的协方差矩阵为：

ψ'＝E[ψ'ψ'^T]＝4B'cov(z_a)B'

(32)

式中，

B'＝diag{x⁰-x₁,y⁰-y₁,r₁⁰}

(33)

z_a'的最小二乘估计为：

Ψ′中的B可用第一次最小而出层估计的结果，则式(34)可化简为：

最终，对待定目标位置的估计：

通过以上过程，即可得到人员位置(x，y)；

S5、使用Savitzky-Golay滤波器进行平滑处理：

使用chan算法解算结果作为Savitzky-Golay滤波器的输入，其详细流程如下：

考虑一组以n＝0为中心的2M+1个数据，用如下的多项式来拟合它，

拟合残差为，

其中，N是多项式阶数，M为单边点数，ε越小则对原数据拟合度越高，根据费马定律，式(38)中ε对各个参数求偏导为0时，ε最小，即

化简后可得，

式(40)就是最小二乘逼近的正规方程，在此方程中需要满足n≤2M的条件；

引进矩阵A(2M+1行，N+1列)，辅助矩阵B(N+1行，N+1列)，使得，

B＝A^TA (41)

定义输入样本值向量X和系数矩阵a为，

则式(40)可以使用下式表示，

Ba＝A^TAa＝A^TX (43)

多项式的系数矩阵a可表示为，

a＝(A^TA)^-1A^TX＝HX，H＝(A^TA)^-1A^T

(44)

式(44)中H的第一行就为所求的卷积系数a₀的值，且S-G滤波器的卷积系数只和滤波器阶数N，单边点数M相关，与被处理何种数据无关；

由于辅助矩阵A展开后有，

当滤波器N增大时，运算量也不断增加，考虑到位置解算对实时性的要求，这里选用1阶滤波器，为了防止过拟合带来的大计算量，选用M为3，根据式(44)和(45)即可得到系数的值，然后对chan解算的位置进行平滑。

2.根据权利要求1所述的一种基于CKF、chan解算和Savitzky-Golay平滑滤波的位置解算方法，其特征在于在实际应用中，定位基站首先测量得到标签到各个基站的时间戳，按照上述算法先进行滤波，进而得到从基站到主基站的到达时间差ΔT；然后使用chan算法进行位置解算；最后再使用Savitzky-Golay对位置轨迹进行平滑。