基于扩展卡尔曼滤波的TDOA与AOA定位方法

1.定位的数学模型参考节点数目为M （3M ≥），对于定位系统可以得到M-1个TDOA 数据和M 个AOA 数据，则可以有如下非线性方程：()1112,3,...,arctan 1,2,...,i i i i i ii d d d v i M y y v i Mx x θθ=-+=⎧⎪⎛⎫⎨-=+= ⎪⎪-⎝⎭⎩()()22i i i d x x y y =-+-；()()22111d x x y y =-+-；1;i i v v θ是观测噪声。

矩阵形式如下：()Y h X V =+()()()12213121213131111111222211211...arctan ;;;arctan ...arctan M M M M M M M M M d d d d v d v d d d y y v d x x x X Y V h X v y y y v x x v y y x x θθθθθθ-⨯-⨯-⎡-⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥-⎢⎥⎢⎥⎢⎥⎛⎫⎢⎥-⎢⎥ ⎪⎢⎥⎡⎤⎢⎥-====⎝⎭⎢⎥⎢⎥⎢⎥⎣⎦⎢⎥⎢⎥⎛⎫-⎢⎥⎢⎥ ⎪-⎢⎥⎝⎭⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎛⎫- ⎪-⎝⎭⎣()211M -⨯⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎦ 噪声向量的协方差矩阵应为()()2121M M -⨯-方阵00R R R τθ⎡⎤=⎢⎥⎣⎦，其中R τ是TDOA观测的方差阵，R θ是AOA 观测的方差阵。

2.非视距识别和TDOA/AOA 混合定位结构流程图TDOA/AOA 混合定位方法的主要思想是使用多种定位方案，通过多样性复和来取得更高的定位精度。

NLOS 识别和TDOA/AOA 混合定位结构图3.NLOS 识别假设参与定位的目标节点数目为M ，目标节点向参考节点发送信号的传播时间为i τ，则距离为：i i r c τ=。

取K 个观察时刻，则第i 个参考节点和目标节点在k t 时刻的距离()i k r t 角度()i k t θ分别如下：()()()()()()()(),,,,,,1,2,...,0,1,....,1i k Los i k rang i k NLos i k i k Los i k bearing i k NLos i k r t r t n t r t i M k K t t n t t θθθ=++⎧⎪==-⎨=++⎪⎩；()(),,,Los i k Los i k r t t θ分别是真实距离与真实角度；()(),,,rang i k bearing i k n t n t 分别是距离的观测噪声和角度的观测噪声；均为零均值的高斯白噪声，方差分别为22,r θσσ()(),,,NLos i k NLos i k r t t θ分别是由NLOS 引起的距离误差（服从指数分布）和角度误差（服从均匀分布）；由NLOS 引起的误差与观测误差的分布不同，且有NLOS 引起的误差对TOA 测量值的影响很大，所以通过卡尔曼滤波器对数据进行处理，使TOA 估计更精确的逼近真实TOA ，以消除有NLOS 引起的误差。

卡尔曼滤波器是一种通过迭代算法实现的估计器，它可以对状态向量做出线性、无偏及最小方差的估计。

用卡尔曼滤波器进行参数估计，关键是找到合适的状态向量及状态向量与观测向量之间的关系。

卡尔曼滤波器的状态方程：11k k k X X W --=Φ+Γ其中：'Tk k k X r r ⎡⎤=⎣⎦是在k t 时刻的状态向量，'k r 表示k r 的径向速度。

1k W -是输入噪声向量，其协方差矩阵为2u Q I σ=Φ是状态转移矩阵，101T ⎡⎤Φ=⎢⎥⎣⎦；Γ是噪声输入矩阵，0T ⎡⎤Γ=⎢⎥⎣⎦；T 为采样间隔。

测量方程为：k k k Y HX V =+其中：k Y 是观测数据向量；[]10H =；k V 为观测噪声序列，其协方差矩阵为2x R I σ=；卡尔曼滤波函数有两个递归步骤组成： 一、时间更新：,11,1k k k k k X X ---=Φ（1）,1,1,1,11,11k k k k T Tk k k k k k k k P P Q -------=ΦΦ+ΓΓ（2） ()1,1,1k k k kT T k k k k k K P H H P H R ---=+（3）二、测量更新：(),1,1k k k k k k k k X X K Y H X --=+-（4）(),1k k k k k P I K H P -=-（5）其中，k K 是卡尔曼滤波增益； k P 是k X 的协方差矩阵；由于无法事先确定待测目标与参与定位的M 个参考节点之间的环境是LOS 还是NLOS ，可以利用历史距离测量值和无偏卡尔曼滤波处理结果周期性的检测待测节点与参考节点之间的LOS/NLOS 传播环境，其中检测周期和误差计算样本值test N 有实验确定。

对于每一个参考节点，通过收集一组test N 个TOA 原始数据周期性的完成LOS/NLOS 的假设测试。

设()i k X t 是第i 个参考节点在k t 时刻经卡尔曼滤波平滑后的距离值，测量距离数据的标准差计算公式如下：()()()211testN i i k i k k testr t X t N σ==-∑（6）在视距环境下，样本标准差将近似等于LOS 环境下的观测噪声方差r σ； 在非视距环境中，此样本将远远大于LOS 环境下的观测噪声方差r σ。

对于超宽带系统，LOS/NLOS 假设测试的判断规则如下：01::i r i rH LOS H NLOS σγσσγσ<>环境环境（7）式中1γ>，值有实验确定，用来消除因误差的随机性而引起的LOS/NLOS 传播判断错误。

4.非视距环境下TOA 误差消除和AOA 信息选择NLOS 误差消除由两部分组成：NLOS 传播环境下的TOA 数据误差消除和AOA 信息的选择。

如果是在LOS 传播环境下，可以采用无偏卡尔曼滤波器对TOA 数据进行平滑；如果是在NLOS 传播环境下，TOA 测量值会发生很大偏差，且由于NLOS 传播引起误差的标准差远远大于观测噪声误差的标准差，此时，将采用有偏卡尔曼滤波消除由NLOS 传播产生的TOA 误差，此外可以通过指定噪声协方差矩阵的对角线元素，来减少TOA 测量值中由非视距传播造成的正向偏差的影响。

噪声协方差对角线元素定义如下：1,1,0,k k x r k r if Y H X and NLOSσασσ++=->=否则其中α的值是由试验确定的。

从所有参考节点得到的TOA 数据均被用于计算TDOA ，TDOA 数据可以用于计算待测节点的位置和对待测节点进行跟踪。

超宽带环境中，有NLOS 引起的距离误差(),NLos i k r t 服从指数分布，设有NLOS 引起的误差使观测噪声k V 的均值[]k E V C =，由11k k k X X W --=Φ+Γ可知k W 是均值为零的白噪声，可以用,11k k k X X --=Φ作为k X 的预测估计，考虑到[]k E V C =，所以对于k 时刻的观测值k Y 的预测估计为：,1,1k k k k Y H X C --=+在k 时刻获得的观测值k Y 与其预测估计值,1k k Y -之间存在误差：,1,1k k k k k k Y Y Y H X C ε--=-=--为了得到k 时刻k X 的滤波值，可以利用预测误差ε来修正原来的状态预测估计,1k k X -，于是有(),1,1k k k k k k k k X X K Y H X C --=+--，其中k K 是卡尔曼滤波增益。

有偏卡尔曼滤波的方程为： 一、时间更新：,11,1k k k k k X X ---=Φ（8）,1,1,1,11,11k k k k T Tk k k k k k k k P P Q -------=ΦΦ+ΓΓ（9） ()1,1,1k k k kT T k k k k k K P H H P H R ---=+（10）二、测量更新：(),1,1k k k k k k k k X X K Y H X C --=+--（11）(),1k k k k k P I K H P -=-（12）其中，k K 是卡尔曼滤波增益； k P 是k X 的协方差矩阵；为了避免引入大的NLOS 角度误差，来自所有参考节点的AOA 信息都要通过AOA 信息选择。

只有那些来自LOS 基站的AOA 数据用于参考节点位置估计，即那些来自NLOS 基站的AOA 数据将被抛弃而不用于待测目标的位置估计。

非视距识别和NLOS 误差消除的仿真步骤：1、令0k =，给定初值：00P X 、； 2、当test k N <时，根据式（1）～（5）重复递推计算，即采用无偏卡尔曼滤波消除噪声，否则直接进行步骤3；3、当test k N >时，根据式（6）计算历史距离测量数据的标准差；4、根据式（7）判断传播环境是LOS 还是NLOS ；5、如果是LOS 则采用第二步中的无偏卡尔曼滤波消除观测噪声，如果是NLOS 则采用式（8）～（12）采用有偏卡尔曼滤波消除观测噪声，并把此时的AOA 观测值抛弃；6、令1k k =+返回第二步，重复递推计算；。