卡尔曼滤波器设计及应用
摘要：卡尔曼滤波器 (Kalman filtering)是一种递归状态估计，即已知上一时刻的状态估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。它提供了一种高效可计算的方法来估计过程的状态，并且使的估计的最小均方差最小。它是一种利用线性系统状态方程，通过系统输入输出的观测数据，对系统的状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。通过对卡尔曼滤波器进行系统的论证和分析，建立以陀螺仪和加速度计测量数据为误差的数学模型,采用卡尔曼滤波的方法,实现数据融合,有效地提高了姿态检测系统的检测精度，并通过以K60单片机为核心的惯性测量单元进行试验验证。结果表明，该算法能够有效避免动态噪声对加速度传感器数据的影响，提高直立角度测量的精度。
关键词: 最优估计 数据融合 卡尔曼滤波 直立角度测量
The Kalman Filter Design and Applied Research
Abstract: Kalman filter is a kind of recursive state estimation, which is known on the moment of the state estimation and the observation of the current state value can be calculated on the estimated value of the current state, so there is no need to record observations or estimation of historical information. It provides a highly efficient method of calculation to estimate the status of the process, and the estimation of the minimum variance minimum. It is a linear state equation of the system, through the data of system input and output, the optimal estimation algorithm of system state. Due to the impact of noise and interference in the system including the observational data, so the optimal estimation can also be seen as a filtering process. Through the demonstration and analysis system of Kalman filter, mathematical model is established based on gyro and accelerometer error, using the method of Kalman filtering, data fusion, effectively improve the detection accuracy of attitude detection system, and test the inertial measurement unit based on K60 microcontroller as the core. The results show that, the algorithm can effectively avoid the influence of dynamic noise of acceleration sensor, improve the measurement accuracy of upright posture.
Key words: optimal estimation; data fusion; Kalman filter; upright attitude measurement
绪论 信号是传递和运输信息的时间或空间的函数，信号在传输和检测过程中不可避免的会受到外来干扰和内部噪声的影响，为获取所需要的信号，排除干扰，就要对信号进行滤波。滤波就是从测量到的或接受到的各种带有干扰的信号中取出有用的信号的方法或技术。
最小均方误差估计是一种小常用的比较简单的经典准则。对于平稳时间序列的最小均方误差的第一个明确的解是维纳在1942年2月首先给出的。维纳滤波器是基于最小均方误差准则的估计器。维纳滤波器理论上是非常完美的，但是实际应用中却遇到了许多的困难。这种算法要求设置大量的存储器来保存过去的测
量数据，计算量大，很难用于实时处理，尤其是军事、航空等领域。为此，许多科技工作者进行了多方探索，但在解决平稳过程的滤波问题时能给出的方法很少。1960年，匈牙利数学科学家卡尔曼发表了一篇关于离散数据线性滤波递推算法的论文，这意味着卡尔曼滤波的诞生。卡尔曼滤波器（KF）是一种纯粹的时域滤波器，它不需要像低通滤波器等频域滤波器那样，需要在频域设计在转换到时域实现。卡尔曼滤波器(Kalman Filter）是一种递归的估计，即已知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。它提供了一种高效可计算的方法来估计过程的状态，并使的估计均方误差最小。卡尔曼滤波器应用广泛，且功能强大，它可以估计信号的过去和当前的状态，甚至能估计将来的状态，即使并不知道模型的确切性质。 1 卡尔曼滤波器（KF）
1.1 滤波器的起源及发展
1960年，匈牙利数学家卡尔曼发表了一篇关于离散数据线性滤波递推算法
的论文，这意味着卡尔曼滤波的诞生。斯坦利施密特(Stanley Schmidt)首次实现了卡尔曼滤波器，卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由是Sweling (1958) Kalman (1960) 与 Kalman and Bucy(1961)发表卡尔曼滤波器是一种具有相当广泛应用的滤波算法，但它只能应用到线性系统中，对于非线性系统有一定的局限性。而且系统中的各个噪声与状态变量都应该呈现出高斯分布，而这两条并不总能限制卡尔曼滤波器在现实生活中的应用。扩展卡尔曼滤波器（EKF）极大地拓宽了卡尔曼滤波的适用范围。EKF的基本思路是，假设卡尔曼滤滤对当前系统状态估计值非常接近于其真实值，于是将非线性函数在当前状态估计值处进行台劳展开并实现线性化。不敏卡尔曼滤波器（UKF）是针对非线性系统的进行的一种改进型卡尔曼滤波器。UKF在处理非线性系统的基本思路在于不敏变换，而不敏变换从根本上讲是一种描述高斯随机变量在非线性下的概率分布情况的方法。不敏卡尔曼滤波认为，与其将一个非线性通过转换成线性化实现近似处理，还不如将高斯随机变量通过非线性变换后，把概率分布情况用高斯分布来近似，所以不敏卡尔曼滤波算法没有实现非线性化这一步骤。不敏卡尔曼滤波器按照一套公式产生一系列采样点，每一采样点均配有一个相应的权重，而这些带有权值的采样点被用来完整地描述系统状态向量估计值的分布图，它们替代了原先卡尔曼滤波器中的状态向量估计值及协方差。不敏卡尔曼滤器让这些样点一一经历非线性状态方程与测量方程，然后再将这些经非线性变换后的样点按照它们的权重而综合出对当前时刻的系统状态向量估计值。多态自适应（MMA）卡尔曼滤波器是一种受到广泛关注的滤波器，它可以根据系统自身的需要，实时进行数据的改变。它由多个并联而且同时经行运行的卡尔曼滤波器组成。在这组卡尔曼滤波器中，每一个滤波器对未知的滤波参数分别做出相互不同的假设，然后各自按照自己的模型假设进行滤波计算，而多态自适应滤波器最后将它们对系统状态的各个估计值进行加权，并以此作为最优估计值输出。
1.2 卡尔曼滤波器原理
1.2.1 卡尔曼滤波器状态空间模型 卡尔曼滤波是基于状态空间方法的一套递推滤波算法，在状态空间方法中，引入了状态变量的概念。实际应用中，可以通过选取合适的状态变量来体现系统的特征、特点和状况的变化。卡尔曼滤波的模型包括状态空间模型和状态观测模型。状态模型是反映状态变化规律的模型，通过状态方程来描写相邻时刻的状态转移变化规律。观测模型反映了实际观测量与状态变量之间的关系，它是实现的状态反馈的重要模型。
首先我们可以假设卡尔曼滤波器用于估计离散时间过程的状态变量X。这个离散时间过程由以下离散随机差分方程描述：
Xk=AXk−1+BUk−1+ Wk (1.1)
定义观测变量Z，得到量测方程：
Zk = HXk+Vk (1.2)
随机信号Wk和Vk分别表示过程激励噪声和观测噪声。它们之间相互独立分布，且符合正态分布的白色噪声：
p(w)~N(0,Q) (1.3)
p(v)~N(0,R) (1.4)
实际系统中，过程激励噪声协方差矩阵Q和观测噪声协方差矩阵R可能会随每次迭代计算而变化。定义 xˆ-
k（− 代表先验，ˆ代表估计）为在已知的第k步的以
前状态情况下第k 步的先验状态估计。定义xˆk为已知测量变量Zk时第k步的后验状态估计。由此定义先验估计误差和后验估计误差：
−eK≡xk−xˆ k-
ek≡xk−
先验估计误差的协方差为： xˆk
−−−Pk=E[ek ek] (1.5) T
后验估计误差的协方差为：
TPk=E[ekek] (1.6)
构造卡尔曼滤波器的表达式：先验估计xˆ
--k和加权的测量变量Zk及其预Hxˆ-k之差的线性组合构成了后验状态估计xˆk： xˆk=xˆ
-K+K(Zk−HxˆK) (1.7) -式中测量变量及其预测之差(Zk−HxˆK) 被称为测量过程的革新或残余。残余反
映了预测值和实际值之间的不一致程度。残余为零则表明二者完全吻合。式1.7中n × m 阶矩阵K叫做残余的增益或混合因数，作用是使1.6式中的后验估计误差协方差最小。可以通过以下步骤计算K ：首先将1.7式代入ek的定义式，再将ek代入1.6式中，求得期望后，将1.6式中的Pk 对K 求导。并使一阶导数为零从而解得K值。K的一种表示形式为：
Kk=
= −T−TPkH（HPkH+R） −1HPkH+RTP−H (1.8)
由1.8式可知，观测噪声协方差R越小，残余的增益越大K越大。特别地，R趋向于零时，有：
Rk→0limKk=H−1
−−另一方面，当先验估计误差协方差Pk越小，残余的增益K越小。特别地，Pk趋向
于零时，有：
P−k→0limKk=0
增益K的另一种解释是随着测量噪声协方差R趋于零，测量变量Zk的权重越来越大，而Zk的预测Hxˆ-
k−的权重越来越小。另一方面，随着先验估计误差协方差Pk趋
于零，测量变量Zk的权重越来越小，而Zk的预测xˆ-
k的权重越来越大。
1.2.2 卡尔曼滤波器工作流程 卡尔曼滤波器是一种用反馈控制的方法估计过程状态：滤波器估计过程某一时刻的状态，然后以（含噪声的）测量变量的方式获得反馈。因此卡尔曼滤波器可分为两个部分：时间更新方程和测量更新方程。时间更新方程负责及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计。测量更新方程负责反馈——也就是说，它将先验估计和新的测量变量结合以构造改进的后验估计。时间更新方程也可视为预估方程，测量更新方程可视为校正方程。最后的估计算法成为一种具有数值解的预估－校正算法。时间更新方程将当前状态变量作为先验估计及时地向前投射到测量更新方程，测量更新方程校正先验估计以获得状态的后验估计，如图1.1所示。
图1.1: 离散卡尔曼滤波器循环更新图
首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态：
X(k|k−1)=A X(k−1|k−1)+B U(k)
(1.9)
式(1.9)中，X(k|k−1)是利用上一状态预测的结果，X(k−1|k−1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k−1)的covariance还没更新。我们用P表示covariance：
P k k−1 =A P k−1 k−1 A，+Q (1.10)
式 (1.10)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k−1|k−1)是X(k−1|k−1)对应的covariance，A，表示A的转置矩阵，Q是系统过程的covariance。式子（1.9）、（1.10）就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态X(k)的最优化估算值:
X(k|k)= X(k|k−1)+Kg(k) (Z(k)−H X(k|k−1)) (1.11)
其中Kg为卡尔曼增益(Kalman Gain)：
Kg(k)= P(k|k−1) H’ / (H P(k|k−1) H’+R) (1.12)
到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：
P(k|k)=（I−Kg(k) H）P(k|k−1) (1.1.3) 其中I为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)是 式子(2)的P(k−1|k−1)。这样，算法就可以自回归的运算下去。
下图1.2显示了滤波器的整个操作流程：
图1.2: 卡尔曼滤波器工作原理图
1.3 滤波器系数调整
滤波器实际实现时，测量噪声协方差R一般是可以观测得到，是滤波器的已知条件。观测测量噪声协方差R一般是可实现的（可能的）
，毕竟我们要观测整
个系统过程。因此通常我们离线获取一些系统观测值以计算测量噪声协方差。通常更难确定过程激励噪声协方差的Q值，因为我们无法直接观测到过程信号xk。有时可以通过Q的选择给过程信号“注入”足够的不确定性来建立一个简单的（差的）过程模型而产生可以接受的结果。当然在这种情况下人们希望信号观测值是可信的。
在这两种情况下，不管我们是否有一个合理的标准来选择系数，我们通常（统计学上的）都可以通过调整滤波器系数来获得更好的性能。调整通常离线进行，并经常与另一个（确定无误的）在线滤波器对比，这个过程称为系统识别。在讨论的结尾，我们指出在Q和R都是常数的条件下，过程估计误差协方差R 和卡尔曼增益Kk 都会快速收敛并保持为常量（参照图1-2中的更新方程）。若实际情况也如此，那么滤波器系数便可以通过预先离线运行滤波器计算.实际中，观测误差R尤其不易保持不变。例如，用我们的光电跟踪仪观察挂在房间顶棚面板上的信号灯时，较近的信号灯会比较远的信号灯具有较小的观测噪声。不仅是观测噪声会变化，有时过程激励噪声协方差Q也会随着滤波器运行而动态变化——这样Q变成了Qk——来适应不同的动态状态。例如，在跟踪三维虚拟环境中用户头部位置时，如果用户头部缓慢移动，我们会减小Qk的幅度，如果移动开始快速变化，则增加幅度。在这些情况下，Qk的幅度要根据用户的移动方向和模型的不确定性来选择。
2卡尔曼滤波器信号融合应用
2.1 系统硬件部分设计
2.1.1 加速度传感器 加速度传感器是可以测量由地球引力作用或者物体运动所产生的加速度。本系统中使用的加速度传感器为飞思卡尔公司产生的MMA7620加速度传感器。该系列的传感器采用了半导体表面微机械加工和集成电路技术，传感器体积小，重量轻。它的基本原理如图2.1所示。
图2.1 加速度传感器
通过微机械加工技术在硅片上加工形成了一个机械悬臂。它与相邻的电极形成了两个电容。由于加速度使得机械悬臂与两个电极之间的距离发生变化，从而改变了两个电容的参数。通过集成的开关电容放大电路量测电容参数的变化，
形成了
与加速度成正比的电压输出。MMA7260是一款三轴低g半导体加速度计，可以同时输出三个方向上的加速度模拟信号，如图2.2所示。
图2.2 三轴加速度传感器
通过设置可以使得MMA7260最大输出灵敏度为800mV/g。只需要测量其中一个方向上的加速度值，就可以计算出车模倾角，比如使用Z轴方向上的加速度信号。
2.1.2 角速度传感器-陀螺仪 陀螺仪可以用来测量物体的旋转角速度。本系统中使用的陀螺仪为EN-03系列的陀螺仪传感器。它利用了旋转坐标系中的物体会受到克里利奥力的原理，在器件中利用压电陶瓷做成振动单元。当旋转器件时会改变振动频率从而反映出物体旋转的角速度。它具有体积小，重量小，响应快，功耗低等优点。在系统中陀螺仪可以测量车模倾斜的角速度，将角速度信号进行积分便可以得到车模的倾角。如图2.3所示。
图2.3 角速度和角度
2.2系统数学建模
2.2.1陀螺仪加速度数学建模 首先对系统进行分析建立系统的数学建模，列出系统的状态方程和测量方程。由于角度和角速度之间存在导数的关系，陀螺仪模块的数据输出得到的是角速度，经过单位时间的积分便得到计算的角度，可作为系统的状态方程。而加速度传感器的数据输出即为测量的角度，可作为系统观测方程。系统的真实倾斜角度∅
用来做一个状态向量。在该系统中，采用加速度计
估计输出跟踪陀螺仪常值偏差量δ，以此偏差作为状态向量得到相应的状态方程和观测方程:
0−1∅1∅ = + wgyro+ wg 00δ0 δ∅∅acce= 1 0 +wa δ
式中， wgyro为包含固定偏差的陀螺仪输出角速度值， ∅acce为加速度计经处理后测量输出得到的角度值， wg为陀螺仪测量噪声， wa 为加速度计测量噪声， δ为陀螺仪漂移误差， wg与wa 之间相互独立，此处假设二者为满足正态分布的白色噪声，令Ts为系统采样周期，得到离散系统的状态方程和测量方程：
T1−TsX k = X k−1 + s wgyro k−1 + Wg(k)Ts 010V k = 1 0 X k +wa k
同时,要估算k时刻的实际角度,就必须利用k-1时刻的角度值,再根据预测得到的k时刻的角度值得到k时刻的高斯噪声的方差，在此基础之上卡尔曼滤波器进行递归运算直至估算出最优的角度值。因此，须知道系统过程噪声协方差阵Q以及测量误差的协方差矩阵R，对卡尔曼滤波器进行校正。Q与R矩阵的形式如下：
q_acce0Q= R= racce 0q_gyro
式中，q_acce 和q_gyro 分别是加速度计和陀螺仪测量的协方差,其数值代表卡尔曼滤波器对其传感器数据的信任程度,值越小，表明信任程度越高.系统中通过手动输入一定的数据来模拟系统的过程噪声，系统中陀螺仪的值更为接近实际真实值，因此取q_gyro的值小于q_acce的值。
2.2.2陀螺仪加速度模块滤波过程 首先确定当前状态为:
X(k|k−1)=AX (k−1|k−1)+BU(k) (2.1)
T1−Ts式中 A= ，B= s 。X(k|k-1)是对k时刻预测的结果即向前的估计值。010
X(k-1|k-1)是k-1时刻的最优结果。则有对应于X(k|k-1)的协方差为：
P(k|k−1)=AP(k−1|k−1)AT +Q (2.2)
式中,P(k-1|k-1)是X(k-1|k-1)对应的协方差，AT 表示A 的转置矩阵,Q 是系统过程的协方差.式子(2.1)、(2.2)即对系统的状态更新。则状态k的最优化估算值X (k|k)：
X(k|k)=X(k|k−1)+K(k)(Z(k)−HX (k|k−1)) (2.3)
其中H =[1 0]，K为卡尔曼增益(Kalman Gain)：
K(k)=P(k|k−1)HT/(HP(k|k−1)HT +R) (2.4)
此时,我们已经得到了k状态下最优的估算值X(k|k)。但是为了使卡尔曼滤波器不断的运行下去直到找到最优的角度值,我们还要更新k状态下X(k|k)的协方差：
P(k|k)=(I−K(k)H )P(k|k−1) (2.5)
其中，I为单位阵,对于本系统则有I=[1 1]，当系统进入k +1 状态时，P(k|k)就是式子(2.2)的P(k-1|k-1)，(2.3)、(2.4)、(2.5)式为卡尔曼滤波器状态更新方程。计算完时间更新方程和测量更新方程后，再次重复上一次计算得到的后验估计，作为
下一次计算的先验估计。这样，周而复始、循环反复地运算下去直至找到最优的结果。
2.3 实验验证
为了验证卡尔曼滤波算法对加速度信号和陀螺仪信号融合方法的有效性，构
表１ 系统实验器件
表２ 滤波器初始化系数
根据卡尔曼滤波原理编写滤波算法程序，通过实际搭建的加速度陀螺仪硬件模块平台，并在IAR嵌入式编程软件中编写程序。经过具体数据的实际调整，得到的实际滤波效果如图2.4，图2.5所示。
图2.4陀螺仪数据融合
图2.5加速度数据融合
2.4 结果分析
图2.4中红色为未融合的陀螺仪数据，蓝色为融合后的陀螺仪数据。通过分析比较陀螺仪原信号输出与处理后的输出可以看出卡尔曼滤波算法即实现了对陀螺仪的滤波作用，又抑制了陀螺仪的温飘问题。这是互补式滤波算法等其他算法所不能有效实现的。
图2.5中黄色为加速度的数据，棕色为融合后的角度。可以看出引入卡尔曼滤波器后加速度计成功融合出实际真实的角度。滤波输出曲线平滑，滤波效果明显，倾角估计的精度得到很大提高。但简单的卡尔曼滤波却不能有效抑制对加速度的温飘问题，自适应卡尔曼滤波则是下一步研究目标。
3总结与展望
文中系统研究分析了经典的卡尔曼滤波器，重点讨论了卡尔曼滤波器的基本原理和算法。然后通过与实际相结合，研究了基于卡尔曼滤波器的加速度计与陀螺仪的信号融合方法，实现了理论在实践上的应用。
在动态数据采集试验的基础上，通过使用卡尔曼滤波算法有效地补偿了传感器的漂移作用与测量噪声等因素对加速度计传感器和陀螺仪传感器的影响，减小了姿态角度测量所带来的误差，提高了运算精度，并成功将该方法灵活应用于直立小车的角度测量系统。实验结果表明了该方法的有效性，顺利实现了小车的直立控制。该方法同样也适用于微小型机器人及四轴飞行器中等系统的姿态角度测量，系统更快速的收敛性将是下一步研究的重点。
致谢
本学位论文是在导师高丽君老师的苦心指导下完成的，在撰写论文的过程中，高丽君老师倾注了大量的心血和汗水，无论是在论文的选题、构思和资料的收集方面，还是在论文的研究方法以及成文定稿方面，都有高老师悉心细致的教诲和无私的帮助，特别是她广博的学识、深厚的学术素养、严谨的治学精神和一丝不苟的工作作风给我留下深刻的印象，使我终生受益。在导师的悉心指导下，我感觉每天都在进步，每一天都能学到一些新的知识，特别是还能学到一些课本以外的知识，这些都使我受益良多。在此表示真诚深深的谢意。
同时感谢我的室友们，从遥远的家来到这个陌生的城市里，是你们和我共同维系着彼此之间兄弟般的感情，维系着寝室那份家的融洽。四年了，仿佛就在昨天。四年里，我们没有红过脸，没有吵过嘴，没有发生上大学前所担心的任何不开心的事情。我们在一起的日子，我会记一辈子的。
特别感谢我的父母他们多年来对我辛苦的抚养和无私的爱，他们的爱护和支持，为我提供了克服一切困难、向前迈进的动力。羊羔跪乳，养育之恩，无以回报，你们永远健康快乐是我最大的心愿。在论文即将完成之际，我的心情无法平静，从开始进入课题到论文的顺利完成，有多少可敬的师长、同学、朋友给了我无私的帮助，在这里请接受我诚挚的谢意。
最后，向在百忙中抽出时间对本文进行评审并提出宝贵意见的各位专家表示衷心地感谢！
参考文献:
[1] 高嵩，潘泉，肖秦琨；传感器自适应滤波融合算法[J]。电子与信息学报,2008,30(8)：1901-1904.
[2]王帅，魏国卡；卡尔曼滤波在四旋翼飞行器姿态测量中的应用。
[3]姚旭晨，卡尔曼滤波器介绍。
[4]Rodrigo da Silva Guerra, Kalman Filter Tutorial for Balancing Robot, April 3, 2008
[5]陈兴林，李鹏，宋申民；基于模糊自适应卡尔曼滤波算法的多传感器信息融合。
[6]潘泉，于昕，程咏梅，张洪才。 信息融合理论的基本方法与进展[J]。 自动化学报，2003，29.
[7]聂博文，马宏绪，王剑等。微小型四旋翼飞行器的研究现状与关键技术．电光与控制，2007，14(6)：113一117．
[8]杨明志，王敏；四旋翼微型飞行器控制系统设计。计算机测量与控制，2008，16(4)：485-487．
[9]刘焕华；小型无人直升机自主飞行控制系统研究.
[10]杨明志，王敏；四旋翼微型飞行器控制系统设计[J]。计算机测量与控制，2008，16(4)：485-487.
[11]周瑞雄，陈梧桐，陈春明等。Kalman Predictor and Mutitarget Tracking Algorithm[J].中华技术学院学报，2003，5(26)：22—36．
附录
/************************************************************************** // Kalman滤波
//**************************************************************************
float angle, angle_dot; //外部需要引用的变量
//-------------------------------------------------------
const float Q_angle=0.05, Q_gyro=0.001, R_angle=5.15, dt=0.005;
// 0.05 0.001 0.001 0.003 0.5,
//注意：dt的取值为kalman滤波器采样时间;
static float P[2][2] = {
{ 1, 0 },
{ 0, 1 }
};
static float Pdot[4] ={0,0,0,0};
static const char C_0 = 1;
static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//-------------------------------------------------------
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{
angle+=(gyro_m-q_bias) * dt;
Pdot[0]=Q_angle- P[0][1] - P[1][0];
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dt;
11
P[0][1] += Pdot[1] * dt; P[1][0] += Pdot[2] * dt; P[1][1] +=Pdot[3]* dt ;
angle_err = angle_m - angle; PCt_0 = C_0 * P[0][0]; PCt_1 = C_0 * P[1][0];
E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0;
t_1 = C_0 * P[0][1];
P[0][0] =P[0][0]-K_0 * t_0; P[0][1] =P[0][1]-K_0 * t_1; P[1][0] =P[1][0]-K_1 * t_0; P[1][1] =P[1][1]-K_1 * t_1; angle += K_0 * angle_err; q_bias += K_1 * angle_err; angle_dot = gyro_m-q_bias; }
12
卡尔曼滤波器设计及应用
摘要：卡尔曼滤波器 (Kalman filtering)是一种递归状态估计，即已知上一时刻的状态估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。它提供了一种高效可计算的方法来估计过程的状态，并且使的估计的最小均方差最小。它是一种利用线性系统状态方程，通过系统输入输出的观测数据，对系统的状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。通过对卡尔曼滤波器进行系统的论证和分析，建立以陀螺仪和加速度计测量数据为误差的数学模型,采用卡尔曼滤波的方法,实现数据融合,有效地提高了姿态检测系统的检测精度，并通过以K60单片机为核心的惯性测量单元进行试验验证。结果表明，该算法能够有效避免动态噪声对加速度传感器数据的影响，提高直立角度测量的精度。
关键词: 最优估计 数据融合 卡尔曼滤波 直立角度测量
The Kalman Filter Design and Applied Research
Abstract: Kalman filter is a kind of recursive state estimation, which is known on the moment of the state estimation and the observation of the current state value can be calculated on the estimated value of the current state, so there is no need to record observations or estimation of historical information. It provides a highly efficient method of calculation to estimate the status of the process, and the estimation of the minimum variance minimum. It is a linear state equation of the system, through the data of system input and output, the optimal estimation algorithm of system state. Due to the impact of noise and interference in the system including the observational data, so the optimal estimation can also be seen as a filtering process. Through the demonstration and analysis system of Kalman filter, mathematical model is established based on gyro and accelerometer error, using the method of Kalman filtering, data fusion, effectively improve the detection accuracy of attitude detection system, and test the inertial measurement unit based on K60 microcontroller as the core. The results show that, the algorithm can effectively avoid the influence of dynamic noise of acceleration sensor, improve the measurement accuracy of upright posture.
Key words: optimal estimation; data fusion; Kalman filter; upright attitude measurement
绪论 信号是传递和运输信息的时间或空间的函数，信号在传输和检测过程中不可避免的会受到外来干扰和内部噪声的影响，为获取所需要的信号，排除干扰，就要对信号进行滤波。滤波就是从测量到的或接受到的各种带有干扰的信号中取出有用的信号的方法或技术。
最小均方误差估计是一种小常用的比较简单的经典准则。对于平稳时间序列的最小均方误差的第一个明确的解是维纳在1942年2月首先给出的。维纳滤波器是基于最小均方误差准则的估计器。维纳滤波器理论上是非常完美的，但是实际应用中却遇到了许多的困难。这种算法要求设置大量的存储器来保存过去的测
量数据，计算量大，很难用于实时处理，尤其是军事、航空等领域。为此，许多科技工作者进行了多方探索，但在解决平稳过程的滤波问题时能给出的方法很少。1960年，匈牙利数学科学家卡尔曼发表了一篇关于离散数据线性滤波递推算法的论文，这意味着卡尔曼滤波的诞生。卡尔曼滤波器（KF）是一种纯粹的时域滤波器，它不需要像低通滤波器等频域滤波器那样，需要在频域设计在转换到时域实现。卡尔曼滤波器(Kalman Filter）是一种递归的估计，即已知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。它提供了一种高效可计算的方法来估计过程的状态，并使的估计均方误差最小。卡尔曼滤波器应用广泛，且功能强大，它可以估计信号的过去和当前的状态，甚至能估计将来的状态，即使并不知道模型的确切性质。 1 卡尔曼滤波器（KF）
1.1 滤波器的起源及发展
1960年，匈牙利数学家卡尔曼发表了一篇关于离散数据线性滤波递推算法
的论文，这意味着卡尔曼滤波的诞生。斯坦利施密特(Stanley Schmidt)首次实现了卡尔曼滤波器，卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由是Sweling (1958) Kalman (1960) 与 Kalman and Bucy(1961)发表卡尔曼滤波器是一种具有相当广泛应用的滤波算法，但它只能应用到线性系统中，对于非线性系统有一定的局限性。而且系统中的各个噪声与状态变量都应该呈现出高斯分布，而这两条并不总能限制卡尔曼滤波器在现实生活中的应用。扩展卡尔曼滤波器（EKF）极大地拓宽了卡尔曼滤波的适用范围。EKF的基本思路是，假设卡尔曼滤滤对当前系统状态估计值非常接近于其真实值，于是将非线性函数在当前状态估计值处进行台劳展开并实现线性化。不敏卡尔曼滤波器（UKF）是针对非线性系统的进行的一种改进型卡尔曼滤波器。UKF在处理非线性系统的基本思路在于不敏变换，而不敏变换从根本上讲是一种描述高斯随机变量在非线性下的概率分布情况的方法。不敏卡尔曼滤波认为，与其将一个非线性通过转换成线性化实现近似处理，还不如将高斯随机变量通过非线性变换后，把概率分布情况用高斯分布来近似，所以不敏卡尔曼滤波算法没有实现非线性化这一步骤。不敏卡尔曼滤波器按照一套公式产生一系列采样点，每一采样点均配有一个相应的权重，而这些带有权值的采样点被用来完整地描述系统状态向量估计值的分布图，它们替代了原先卡尔曼滤波器中的状态向量估计值及协方差。不敏卡尔曼滤器让这些样点一一经历非线性状态方程与测量方程，然后再将这些经非线性变换后的样点按照它们的权重而综合出对当前时刻的系统状态向量估计值。多态自适应（MMA）卡尔曼滤波器是一种受到广泛关注的滤波器，它可以根据系统自身的需要，实时进行数据的改变。它由多个并联而且同时经行运行的卡尔曼滤波器组成。在这组卡尔曼滤波器中，每一个滤波器对未知的滤波参数分别做出相互不同的假设，然后各自按照自己的模型假设进行滤波计算，而多态自适应滤波器最后将它们对系统状态的各个估计值进行加权，并以此作为最优估计值输出。
1.2 卡尔曼滤波器原理
1.2.1 卡尔曼滤波器状态空间模型 卡尔曼滤波是基于状态空间方法的一套递推滤波算法，在状态空间方法中，引入了状态变量的概念。实际应用中，可以通过选取合适的状态变量来体现系统的特征、特点和状况的变化。卡尔曼滤波的模型包括状态空间模型和状态观测模型。状态模型是反映状态变化规律的模型，通过状态方程来描写相邻时刻的状态转移变化规律。观测模型反映了实际观测量与状态变量之间的关系，它是实现的状态反馈的重要模型。
首先我们可以假设卡尔曼滤波器用于估计离散时间过程的状态变量X。这个离散时间过程由以下离散随机差分方程描述：
Xk=AXk−1+BUk−1+ Wk (1.1)
定义观测变量Z，得到量测方程：
Zk = HXk+Vk (1.2)
随机信号Wk和Vk分别表示过程激励噪声和观测噪声。它们之间相互独立分布，且符合正态分布的白色噪声：
p(w)~N(0,Q) (1.3)
p(v)~N(0,R) (1.4)
实际系统中，过程激励噪声协方差矩阵Q和观测噪声协方差矩阵R可能会随每次迭代计算而变化。定义 xˆ-
k（− 代表先验，ˆ代表估计）为在已知的第k步的以
前状态情况下第k 步的先验状态估计。定义xˆk为已知测量变量Zk时第k步的后验状态估计。由此定义先验估计误差和后验估计误差：
−eK≡xk−xˆ k-
ek≡xk−
先验估计误差的协方差为： xˆk
−−−Pk=E[ek ek] (1.5) T
后验估计误差的协方差为：
TPk=E[ekek] (1.6)
构造卡尔曼滤波器的表达式：先验估计xˆ
--k和加权的测量变量Zk及其预Hxˆ-k之差的线性组合构成了后验状态估计xˆk： xˆk=xˆ
-K+K(Zk−HxˆK) (1.7) -式中测量变量及其预测之差(Zk−HxˆK) 被称为测量过程的革新或残余。残余反
映了预测值和实际值之间的不一致程度。残余为零则表明二者完全吻合。式1.7中n × m 阶矩阵K叫做残余的增益或混合因数，作用是使1.6式中的后验估计误差协方差最小。可以通过以下步骤计算K ：首先将1.7式代入ek的定义式，再将ek代入1.6式中，求得期望后，将1.6式中的Pk 对K 求导。并使一阶导数为零从而解得K值。K的一种表示形式为：
Kk=
= −T−TPkH（HPkH+R） −1HPkH+RTP−H (1.8)
由1.8式可知，观测噪声协方差R越小，残余的增益越大K越大。特别地，R趋向于零时，有：
Rk→0limKk=H−1
−−另一方面，当先验估计误差协方差Pk越小，残余的增益K越小。特别地，Pk趋向
于零时，有：
P−k→0limKk=0
增益K的另一种解释是随着测量噪声协方差R趋于零，测量变量Zk的权重越来越大，而Zk的预测Hxˆ-
k−的权重越来越小。另一方面，随着先验估计误差协方差Pk趋
于零，测量变量Zk的权重越来越小，而Zk的预测xˆ-
k的权重越来越大。
1.2.2 卡尔曼滤波器工作流程 卡尔曼滤波器是一种用反馈控制的方法估计过程状态：滤波器估计过程某一时刻的状态，然后以（含噪声的）测量变量的方式获得反馈。因此卡尔曼滤波器可分为两个部分：时间更新方程和测量更新方程。时间更新方程负责及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计。测量更新方程负责反馈——也就是说，它将先验估计和新的测量变量结合以构造改进的后验估计。时间更新方程也可视为预估方程，测量更新方程可视为校正方程。最后的估计算法成为一种具有数值解的预估－校正算法。时间更新方程将当前状态变量作为先验估计及时地向前投射到测量更新方程，测量更新方程校正先验估计以获得状态的后验估计，如图1.1所示。
图1.1: 离散卡尔曼滤波器循环更新图
首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态：
X(k|k−1)=A X(k−1|k−1)+B U(k)
(1.9)
式(1.9)中，X(k|k−1)是利用上一状态预测的结果，X(k−1|k−1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k−1)的covariance还没更新。我们用P表示covariance：
P k k−1 =A P k−1 k−1 A，+Q (1.10)
式 (1.10)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k−1|k−1)是X(k−1|k−1)对应的covariance，A，表示A的转置矩阵，Q是系统过程的covariance。式子（1.9）、（1.10）就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态X(k)的最优化估算值:
X(k|k)= X(k|k−1)+Kg(k) (Z(k)−H X(k|k−1)) (1.11)
其中Kg为卡尔曼增益(Kalman Gain)：
Kg(k)= P(k|k−1) H’ / (H P(k|k−1) H’+R) (1.12)
到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：
P(k|k)=（I−Kg(k) H）P(k|k−1) (1.1.3) 其中I为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)是 式子(2)的P(k−1|k−1)。这样，算法就可以自回归的运算下去。
下图1.2显示了滤波器的整个操作流程：
图1.2: 卡尔曼滤波器工作原理图
1.3 滤波器系数调整
滤波器实际实现时，测量噪声协方差R一般是可以观测得到，是滤波器的已知条件。观测测量噪声协方差R一般是可实现的（可能的）
，毕竟我们要观测整
个系统过程。因此通常我们离线获取一些系统观测值以计算测量噪声协方差。通常更难确定过程激励噪声协方差的Q值，因为我们无法直接观测到过程信号xk。有时可以通过Q的选择给过程信号“注入”足够的不确定性来建立一个简单的（差的）过程模型而产生可以接受的结果。当然在这种情况下人们希望信号观测值是可信的。
在这两种情况下，不管我们是否有一个合理的标准来选择系数，我们通常（统计学上的）都可以通过调整滤波器系数来获得更好的性能。调整通常离线进行，并经常与另一个（确定无误的）在线滤波器对比，这个过程称为系统识别。在讨论的结尾，我们指出在Q和R都是常数的条件下，过程估计误差协方差R 和卡尔曼增益Kk 都会快速收敛并保持为常量（参照图1-2中的更新方程）。若实际情况也如此，那么滤波器系数便可以通过预先离线运行滤波器计算.实际中，观测误差R尤其不易保持不变。例如，用我们的光电跟踪仪观察挂在房间顶棚面板上的信号灯时，较近的信号灯会比较远的信号灯具有较小的观测噪声。不仅是观测噪声会变化，有时过程激励噪声协方差Q也会随着滤波器运行而动态变化——这样Q变成了Qk——来适应不同的动态状态。例如，在跟踪三维虚拟环境中用户头部位置时，如果用户头部缓慢移动，我们会减小Qk的幅度，如果移动开始快速变化，则增加幅度。在这些情况下，Qk的幅度要根据用户的移动方向和模型的不确定性来选择。
2卡尔曼滤波器信号融合应用
2.1 系统硬件部分设计
2.1.1 加速度传感器 加速度传感器是可以测量由地球引力作用或者物体运动所产生的加速度。本系统中使用的加速度传感器为飞思卡尔公司产生的MMA7620加速度传感器。该系列的传感器采用了半导体表面微机械加工和集成电路技术，传感器体积小，重量轻。它的基本原理如图2.1所示。
图2.1 加速度传感器
通过微机械加工技术在硅片上加工形成了一个机械悬臂。它与相邻的电极形成了两个电容。由于加速度使得机械悬臂与两个电极之间的距离发生变化，从而改变了两个电容的参数。通过集成的开关电容放大电路量测电容参数的变化，
形成了
与加速度成正比的电压输出。MMA7260是一款三轴低g半导体加速度计，可以同时输出三个方向上的加速度模拟信号，如图2.2所示。
图2.2 三轴加速度传感器
通过设置可以使得MMA7260最大输出灵敏度为800mV/g。只需要测量其中一个方向上的加速度值，就可以计算出车模倾角，比如使用Z轴方向上的加速度信号。
2.1.2 角速度传感器-陀螺仪 陀螺仪可以用来测量物体的旋转角速度。本系统中使用的陀螺仪为EN-03系列的陀螺仪传感器。它利用了旋转坐标系中的物体会受到克里利奥力的原理，在器件中利用压电陶瓷做成振动单元。当旋转器件时会改变振动频率从而反映出物体旋转的角速度。它具有体积小，重量小，响应快，功耗低等优点。在系统中陀螺仪可以测量车模倾斜的角速度，将角速度信号进行积分便可以得到车模的倾角。如图2.3所示。
图2.3 角速度和角度
2.2系统数学建模
2.2.1陀螺仪加速度数学建模 首先对系统进行分析建立系统的数学建模，列出系统的状态方程和测量方程。由于角度和角速度之间存在导数的关系，陀螺仪模块的数据输出得到的是角速度，经过单位时间的积分便得到计算的角度，可作为系统的状态方程。而加速度传感器的数据输出即为测量的角度，可作为系统观测方程。系统的真实倾斜角度∅
用来做一个状态向量。在该系统中，采用加速度计
估计输出跟踪陀螺仪常值偏差量δ，以此偏差作为状态向量得到相应的状态方程和观测方程:
0−1∅1∅ = + wgyro+ wg 00δ0 δ∅∅acce= 1 0 +wa δ
式中， wgyro为包含固定偏差的陀螺仪输出角速度值， ∅acce为加速度计经处理后测量输出得到的角度值， wg为陀螺仪测量噪声， wa 为加速度计测量噪声， δ为陀螺仪漂移误差， wg与wa 之间相互独立，此处假设二者为满足正态分布的白色噪声，令Ts为系统采样周期，得到离散系统的状态方程和测量方程：
T1−TsX k = X k−1 + s wgyro k−1 + Wg(k)Ts 010V k = 1 0 X k +wa k
同时,要估算k时刻的实际角度,就必须利用k-1时刻的角度值,再根据预测得到的k时刻的角度值得到k时刻的高斯噪声的方差，在此基础之上卡尔曼滤波器进行递归运算直至估算出最优的角度值。因此，须知道系统过程噪声协方差阵Q以及测量误差的协方差矩阵R，对卡尔曼滤波器进行校正。Q与R矩阵的形式如下：
q_acce0Q= R= racce 0q_gyro
式中，q_acce 和q_gyro 分别是加速度计和陀螺仪测量的协方差,其数值代表卡尔曼滤波器对其传感器数据的信任程度,值越小，表明信任程度越高.系统中通过手动输入一定的数据来模拟系统的过程噪声，系统中陀螺仪的值更为接近实际真实值，因此取q_gyro的值小于q_acce的值。
2.2.2陀螺仪加速度模块滤波过程 首先确定当前状态为:
X(k|k−1)=AX (k−1|k−1)+BU(k) (2.1)
T1−Ts式中 A= ，B= s 。X(k|k-1)是对k时刻预测的结果即向前的估计值。010
X(k-1|k-1)是k-1时刻的最优结果。则有对应于X(k|k-1)的协方差为：
P(k|k−1)=AP(k−1|k−1)AT +Q (2.2)
式中,P(k-1|k-1)是X(k-1|k-1)对应的协方差，AT 表示A 的转置矩阵,Q 是系统过程的协方差.式子(2.1)、(2.2)即对系统的状态更新。则状态k的最优化估算值X (k|k)：
X(k|k)=X(k|k−1)+K(k)(Z(k)−HX (k|k−1)) (2.3)
其中H =[1 0]，K为卡尔曼增益(Kalman Gain)：
K(k)=P(k|k−1)HT/(HP(k|k−1)HT +R) (2.4)
此时,我们已经得到了k状态下最优的估算值X(k|k)。但是为了使卡尔曼滤波器不断的运行下去直到找到最优的角度值,我们还要更新k状态下X(k|k)的协方差：
P(k|k)=(I−K(k)H )P(k|k−1) (2.5)
其中，I为单位阵,对于本系统则有I=[1 1]，当系统进入k +1 状态时，P(k|k)就是式子(2.2)的P(k-1|k-1)，(2.3)、(2.4)、(2.5)式为卡尔曼滤波器状态更新方程。计算完时间更新方程和测量更新方程后，再次重复上一次计算得到的后验估计，作为
下一次计算的先验估计。这样，周而复始、循环反复地运算下去直至找到最优的结果。
2.3 实验验证
为了验证卡尔曼滤波算法对加速度信号和陀螺仪信号融合方法的有效性，构
表１ 系统实验器件
表２ 滤波器初始化系数
根据卡尔曼滤波原理编写滤波算法程序，通过实际搭建的加速度陀螺仪硬件模块平台，并在IAR嵌入式编程软件中编写程序。经过具体数据的实际调整，得到的实际滤波效果如图2.4，图2.5所示。
图2.4陀螺仪数据融合
图2.5加速度数据融合
2.4 结果分析
图2.4中红色为未融合的陀螺仪数据，蓝色为融合后的陀螺仪数据。通过分析比较陀螺仪原信号输出与处理后的输出可以看出卡尔曼滤波算法即实现了对陀螺仪的滤波作用，又抑制了陀螺仪的温飘问题。这是互补式滤波算法等其他算法所不能有效实现的。
图2.5中黄色为加速度的数据，棕色为融合后的角度。可以看出引入卡尔曼滤波器后加速度计成功融合出实际真实的角度。滤波输出曲线平滑，滤波效果明显，倾角估计的精度得到很大提高。但简单的卡尔曼滤波却不能有效抑制对加速度的温飘问题，自适应卡尔曼滤波则是下一步研究目标。
3总结与展望
文中系统研究分析了经典的卡尔曼滤波器，重点讨论了卡尔曼滤波器的基本原理和算法。然后通过与实际相结合，研究了基于卡尔曼滤波器的加速度计与陀螺仪的信号融合方法，实现了理论在实践上的应用。
在动态数据采集试验的基础上，通过使用卡尔曼滤波算法有效地补偿了传感器的漂移作用与测量噪声等因素对加速度计传感器和陀螺仪传感器的影响，减小了姿态角度测量所带来的误差，提高了运算精度，并成功将该方法灵活应用于直立小车的角度测量系统。实验结果表明了该方法的有效性，顺利实现了小车的直立控制。该方法同样也适用于微小型机器人及四轴飞行器中等系统的姿态角度测量，系统更快速的收敛性将是下一步研究的重点。
致谢
本学位论文是在导师高丽君老师的苦心指导下完成的，在撰写论文的过程中，高丽君老师倾注了大量的心血和汗水，无论是在论文的选题、构思和资料的收集方面，还是在论文的研究方法以及成文定稿方面，都有高老师悉心细致的教诲和无私的帮助，特别是她广博的学识、深厚的学术素养、严谨的治学精神和一丝不苟的工作作风给我留下深刻的印象，使我终生受益。在导师的悉心指导下，我感觉每天都在进步，每一天都能学到一些新的知识，特别是还能学到一些课本以外的知识，这些都使我受益良多。在此表示真诚深深的谢意。
同时感谢我的室友们，从遥远的家来到这个陌生的城市里，是你们和我共同维系着彼此之间兄弟般的感情，维系着寝室那份家的融洽。四年了，仿佛就在昨天。四年里，我们没有红过脸，没有吵过嘴，没有发生上大学前所担心的任何不开心的事情。我们在一起的日子，我会记一辈子的。
特别感谢我的父母他们多年来对我辛苦的抚养和无私的爱，他们的爱护和支持，为我提供了克服一切困难、向前迈进的动力。羊羔跪乳，养育之恩，无以回报，你们永远健康快乐是我最大的心愿。在论文即将完成之际，我的心情无法平静，从开始进入课题到论文的顺利完成，有多少可敬的师长、同学、朋友给了我无私的帮助，在这里请接受我诚挚的谢意。
最后，向在百忙中抽出时间对本文进行评审并提出宝贵意见的各位专家表示衷心地感谢！
参考文献:
[1] 高嵩，潘泉，肖秦琨；传感器自适应滤波融合算法[J]。电子与信息学报,2008,30(8)：1901-1904.
[2]王帅，魏国卡；卡尔曼滤波在四旋翼飞行器姿态测量中的应用。
[3]姚旭晨，卡尔曼滤波器介绍。
[4]Rodrigo da Silva Guerra, Kalman Filter Tutorial for Balancing Robot, April 3, 2008
[5]陈兴林，李鹏，宋申民；基于模糊自适应卡尔曼滤波算法的多传感器信息融合。
[6]潘泉，于昕，程咏梅，张洪才。 信息融合理论的基本方法与进展[J]。 自动化学报，2003，29.
[7]聂博文，马宏绪，王剑等。微小型四旋翼飞行器的研究现状与关键技术．电光与控制，2007，14(6)：113一117．
[8]杨明志，王敏；四旋翼微型飞行器控制系统设计。计算机测量与控制，2008，16(4)：485-487．
[9]刘焕华；小型无人直升机自主飞行控制系统研究.
[10]杨明志，王敏；四旋翼微型飞行器控制系统设计[J]。计算机测量与控制，2008，16(4)：485-487.
[11]周瑞雄，陈梧桐，陈春明等。Kalman Predictor and Mutitarget Tracking Algorithm[J].中华技术学院学报，2003，5(26)：22—36．
附录
/************************************************************************** // Kalman滤波
//**************************************************************************
float angle, angle_dot; //外部需要引用的变量
//-------------------------------------------------------
const float Q_angle=0.05, Q_gyro=0.001, R_angle=5.15, dt=0.005;
// 0.05 0.001 0.001 0.003 0.5,
//注意：dt的取值为kalman滤波器采样时间;
static float P[2][2] = {
{ 1, 0 },
{ 0, 1 }
};
static float Pdot[4] ={0,0,0,0};
static const char C_0 = 1;
static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//-------------------------------------------------------
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{
angle+=(gyro_m-q_bias) * dt;
Pdot[0]=Q_angle- P[0][1] - P[1][0];
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro;
P[0][0] += Pdot[0] * dt;
11
P[0][1] += Pdot[1] * dt; P[1][0] += Pdot[2] * dt; P[1][1] +=Pdot[3]* dt ;
angle_err = angle_m - angle; PCt_0 = C_0 * P[0][0]; PCt_1 = C_0 * P[1][0];
E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0;
t_1 = C_0 * P[0][1];
P[0][0] =P[0][0]-K_0 * t_0; P[0][1] =P[0][1]-K_0 * t_1; P[1][0] =P[1][0]-K_1 * t_0; P[1][1] =P[1][1]-K_1 * t_1; angle += K_0 * angle_err; q_bias += K_1 * angle_err; angle_dot = gyro_m-q_bias; }
12