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

1.定位的数学模型 参考节点数目为M（），对于定位系统可以得到M-1个TDOA数据和M个AOA数据，则可以有如下非线性方程：；是观测噪声。矩阵形式如下：噪声向量的协方差矩阵应为方阵，其中是TDOA观测的方差阵，是AOA观测的方差阵。2. 非视距识别和TDOA/AOA混合定位结构流程图TDOA/AOA混合定位方法的主要思想是使用多种定位方案，通过多样性复和来取得更高的定位精度。NLOS识别和TDOA/AOA混合定位结构图3. NLOS识别假设参与定位的目标节点数目为M，目标节点向参考节点发送信号的传播时间为，则距离为：。取K个观察时刻，则第i个参考节点和目标节点在时刻的距离角度分别如下：分别是真实距离与真实角度；分别是距离的观测噪声和角度的观测噪声；均为零均值的高斯白噪声，方差分别为分别是由NLOS引起的距离误差（服从指数分布）和角度误差（服从均匀分布）；由NLOS引起的误差与观测误差的分布不同，且有NLOS引起的误差对TOA测量值的影响很大，所以通过卡尔曼滤波器对数据进行处理，使TOA估计更精确的逼近真实TOA，以消除有NLOS引起的误差。卡尔曼滤波器是一种通过迭代算法实现的估计器，它可以对状态向量做出线性、无偏及最小方差的估计。用卡尔曼滤波器进行参数估计，关键是找到合适的状态向量及状态向量与观测向量之间的关系。卡尔曼滤波器的状态方程：其中：是在时刻的状态向量，表示的径向速度。是输入噪声向量，其协方差矩阵为是状态转移矩阵，；是噪声输入矩阵，；T为采样间隔。测量方程为：其中：是观测数据向量；为观测噪声序列，其协方差矩阵为；卡尔曼滤波函数有两个递归步骤组成：一、时间更新：（1）（2）（3） 二、测量更新： （4）（5）其中，是卡尔曼滤波增益；是的协方差矩阵；由于无法事先确定待测目标与参与定位的M个参考节点之间的环境是LOS还是NLOS，可以利用历史距离测量值和无偏卡尔曼滤波处理结果周期性的检测待测节点与参考节点之间的LOS/NLOS传播环境，其中检测周期和误差计算样本值有实验确定。对于每一个参考节点，通过收集一组个TOA原始数据周期性的完成LOS/NLOS的假设测试。设是第i个参考节点在时刻经卡尔曼滤波平滑后的距离值，测量距离数据的标准差计算公式如下：（6）在视距环境下，样本标准差将近似等于LOS环境下的观测噪声方差；在非视距环境中，此样本将远远大于LOS环境下的观测噪声方差。对于超宽带系统，LOS/NLOS假设测试的判断规则如下：（7）式中，值有实验确定，用来消除因误差的随机性而引起的LOS/NLOS传播判断错误。4. 非视距环境下TOA误差消除和AOA信息选择NLOS误差消除由两部分组成：NLOS传播环境下的TOA数据误差消除和AOA信息的选择。如果是在LOS传播环境下，可以采用无偏卡尔曼滤波器对TOA数据进行平滑；如果是在NLOS传播环境下，TOA测量值会发生很大偏差，且由于NLOS传播引起误差的标准差远远大于观测噪声误差的标准差，此时，将采用有偏卡尔曼滤波消除由NLOS传播产生的TOA误差，此外可以通过指定噪声协方差矩阵的对角线元素，来减少TOA测量值中由非视距传播造成的正向偏差的影响。噪声协方差对角线元素定义如下：其中的值是由试验确定的。从所有参考节点得到的TOA数据均被用于计算TDOA，TDOA数据可以用于计算待测节点的位置和对待测节点进行跟踪。超宽带环境中，有NLOS引起的距离误差服从指数分布，设有NLOS引起的误差使观测噪声的均值，由可知是均值为零的白噪声，可以用作为的预测估计，考虑到，所以对于k时刻的观测值的预测估计为：在k时刻获得的观测值与其预测估计值之间存在误差：为了得到k时刻的滤波值，可以利用预测误差来修正原来的状态预测估计，于是有，其中是卡尔曼滤波增益。有偏卡尔曼滤波的方程为：一、时间更新：（8）（9）（10） 二、测量更新：（11）（12）其中，是卡尔曼滤波增益；是的协方差矩阵；为了避免引入大的NLOS角度误差，来自所有参考节点的AOA信息都要通过AOA信息选择。只有那些来自LOS基站的AOA数据用于参考节点位置估计，即那些来自NLOS基站的AOA数据将被抛弃而不用于待测目标的位置估计。非视距识别和NLOS误差消除的仿真步骤：1、 令，给定初值：；2、 当时，根据式（1）（5）重复递推计算，即采用无偏卡尔曼滤波消除噪声，否则直接进行步骤3；3、 当时，根据式（6）计算历史距离测量数据的标准差；4、 根据式（7）判断传播环境是LOS还是NLO