《测试信号处理》读书报告题目：基于最小二乘—卡尔曼滤波的wMPS系统跟踪定位算法研究姓名：天津大学二〇一五年六月一、什么是卡尔曼滤波从分类上讲，卡尔曼滤波属于现代滤波。

卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。

由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。

卡尔曼在NASA 埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。

关于这种滤波器的论文由Swerling (1958), Kalman (1960)与Kalman and Bucy (1961)发表。

数据滤波是去除噪声还原真实数据的一种数据处理技术，Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态。

由于，它便于计算机编程实现，并能够对现场采集的数据进行实时的更新和处理，Kalman滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。

传统的滤波方法，只能是在有用信号与噪声具有不同频带的条件下才能实现。

20世纪40年代，N．维纳和A．H．柯尔莫哥罗夫把信号和噪声的统计性质引进了滤波理论，在假设信号和噪声都是平稳过程的条件下，利用最优化方法对信号真值进行估计，达到滤波目的，从而在概念上与传统的滤波方法联系起来，被称为维纳滤波。

这种方法要求信号和噪声都必须是以平稳过程为条件。

60年代初，卡尔曼(R．E．Kalman)和布塞(R．S．Bucy)发表了一篇重要的论文《线性滤波和预测理论的新成果》，提出了一种新的线性滤波和预测理由论，被称之为卡尔曼滤波。

特点是在线性状态空间表示的基础上对有噪声的输入和观测信号进行处理，求取系统状态或真实信号。

这种理论是在时间域上来表述的，基本的概念是：在线性系统的状态空间表示基础上，从输出和输入观测数据求系统状态的最优估计。

这里所说的系统状态，是总结系统所有过去的输入和扰动对系统的作用的最小参数的集合，知道了系统的状态就能够与未来的输入与系统的扰动一起确定系统的整个行为。

卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件。

对于每个时刻的系统扰动和观测误差（即噪声），只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误差为最小的真实信号的估计值。

因此，自从卡尔曼滤波理论问世以来，在通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等许多部门都得到了应用，取得了许多成功应用的成果。

例如在图像处理方面，应用卡尔曼滤波对由于某些噪声影响而造成模糊的图像进行复原。

在对噪声作了某些统计性质的假定后，就可以用卡尔曼的算法以递推的方式从模糊图像中得到均方差最小的真实图像，使模糊的图像得到复原。

二、 wMPS 系统组成wMPS 三维定位系统主要由激光发射器网络、位置传感器、中心计算机和无线通讯系统组成。

激光发射器由固定基座和转动头组成，安装有两个一字线激光器和一个脉冲激光器，两个一字线激光器固定于转动头上，激光器产生的光平面分别与垂直方向呈±30°，呈V 字形。

当发射器工作时，激光器所产生的光平面随动头一同旋转，对测量空间进行扫描。

脉冲激光器用于产生一个计时同步时刻，以该时刻光平面1与发射器水平面之间的交线为发射器X 正方向，旋转轴为Z 方向，按右手定则确定Y 方向。

当系统工作时，在工件装配的特定工作空间中的不同区域放置多个发射器( Transmitter)，接收器( Receiver) 安装在工件的关键点上，由于发射器与接收器之间的光信号通信是单向广播式( One-Way Broadcast-Style) 的，因此多个接收器可以共用这些发射器信号，如图1所示。

传感器通过计算与两个（以上）发射器的方位角，通过空间交会原理即可得到传感器的空间三维坐标。

图1wMPS 三维坐标系统组成三、 基于最小二乘原理的静态定位方法wMPS 系统的静态定位通常采用基于多面交会的最小二乘估计方法。

如图2( a) 所示，发射器可以抽象为围绕旋转轴以角速度ω旋转的两个光平面。

以初始时刻，光平面1与发射器水平面之间的交线为发射器X 正方向，旋转轴为Z 方向，按右手定则确定Y 方向。

转台位于初始位置时，对两个光平面进行标定，得到两个光平面的方程为：11111111112112112112()()()0()()()0a x x b y y c z z a x x b y y c z z d -+-+-=⎧⎨-+-+-+=⎩ 即11111111111111112121211211( )0( )0x x x x y y a b c y y z z z z x x x x y y a b c y y d z z z z ⎧--⎛⎫⎛⎫⎪ ⎪ ⎪-=-=⎪ ⎪ ⎪ ⎪ ⎪⎪--⎪⎝⎭⎝⎭⎨--⎛⎫⎛⎫⎪ ⎪ ⎪⎪-=-+= ⎪ ⎪⎪ ⎪ ⎪--⎪⎝⎭⎝⎭⎩1112n n 其中，111111( )a b c =11n ，121212( )a b c =12n 分别为光平面1和光平面2的法矢量。

图2wMPS 测量系统原理当光平面1扫过被测点P 时，平面1的法矢量'11n 为'111111'11111111'1111111111111111111111cos sin 0()sin cos 0001cos sin =sin cos a a b R b c c a b a b c θθθθθθθθθ⎡⎤⎡⎤-⎡⎤⎢⎥⎢⎥⎢⎥==⋅=⨯⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦-⎛⎫ ⎪+ ⎪ ⎪⎝⎭'1111n n其中，111t θω=⋅为光平面1转过的角度。

同理可得，光平面2扫过被测点P 时，光平面2的法矢量'12n 为'1212121212'1212121212'1212cos sin =sin cos a a b b a b c c θθθθ⎡⎤-⎛⎫⎢⎥ ⎪=+⎢⎥ ⎪ ⎪⎢⎥⎝⎭⎣⎦'12n若被测点P 的坐标为(,y,z)x ，则可得方程组'''111111111'''12112112112()()()0()()()0a x xb y yc z z a x x b y y c z zd ⎧-+-+-=⎪⎨-+-+-+=⎪⎩ 对于多个发射器，将其方程组改写成矩阵形式可得Ax =b其中，'''111111'''121212'''111'''222 N N N N N N a b c a b c a b c a b c ⎡⎤⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦A ，'''111111111'''12112112112'''111111'''2121212 N N N N N N N a x b y c z a x b y c z d a x b y c z a x b y c z d ⎡⎤++⎢⎥++-⎢⎥⎢⎥=⎢⎥⎢⎥++⎢⎥++-⎢⎥⎣⎦b 其最小二乘解为x =T -1T (A A)A b利用最小二乘法解算接收器的坐标只利用了当前的观测量，不能对观测量进行误差分析，因此解算结果受观测量影响较大，当观测量误差较大或接收器运动时，解算结果精度不高，但最小二乘法收敛速度很快，受初始位置误差影响较小。

四、 基于最小二乘—卡尔曼滤波的动态测量方法如果直接利用接收器测得的与多个发射器之间的方位角信息进行卡尔曼滤波跟踪定位，不可避免要解决非线性估计问题，这将使算法复杂，同时引入非线性误差，降低解算精度。

此外，随着观测量维数的增加，计算量也大幅增加，因此我们利用静态坐标解算的方法对接收器坐标进行估计，将其结果作为伪测量值，然后在利用匀速运动模型进行线性卡尔曼滤波，实现高精度定位跟踪。

1 运动方程和测量方程在数字化装配过程中，工件所做的多为匀速运动、匀加速运动以及低速转弯等机动性较弱的运动，因此选用匀速运动模型建立运动方程，将加速度作为运动噪声。

设状态变量[,,,,,]T x y z x y z v v v =X ，其中,,x y z 分别为接收器的空间三维坐标，,,x y z v v v 分别为接收器运动速度在,,x y z 方向上的分量，采样间隔为T 。

状态转移方程为：|1111k k k k k k X X G W ----=Φ+其中，状态转移矩阵、状态转移噪声矩阵分别为|1100000100000100000100000010000001k k T T T -⎡⎤⎢⎥⎢⎥⎢⎥Φ=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦2221/2000/2000/2000000K T T T G T T T -⎡⎤⎢⎥⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦状态转移噪声向量为1111[]k k k k w wx wy wz ----=同时，状态转移噪声满足112111111()0(()())k k T T k k k k k k E G w E G w G w G G a --------==系统的观测量分别为接收器的坐标(,,)x y z ，观测方程为k k k k Z H X V =+其中，观测矩阵为100000010000001000k H ⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦观测噪声满足22200()0,()0000x T k k k y z E V E V V σσσ⎡⎤⎢⎥==⎢⎥⎢⎥⎣⎦2 线性卡尔曼滤波采用最小二乘计算所得的坐标作为伪观测量，系统转移方程和观测方程均为线性，采用卡尔曼滤波对测量结果进行最优化估计。

卡尔曼滤波步骤如下：（1）计算状态变量及其协方差初值。

1,2k k ==时，直接利用观测值12,Z Z 通过最小二乘原理计算出接收器的位置分别为111(,,)x y z 和222(,,)x y z 。

求出系统状态变量初始值为[]2222212121TX x y z x x y y z z =--- 2(0.040.040.040.250.250.25)P diag = （2）利用状态转移方程及前一步的状态变量对当前状态进行最优估值。

当3k ≥时，系统状态变量|1k k X -和方差|1k k P -的先验估计为：|1|11|1|1|11|1|11k k k k k k Tk k k k k k k k k X X P P Q ----------=Φ=ΦΦ+（3）通过观测量对估值进行修正。