基于时间序列分析的卡尔曼滤波组合导航算法
第26卷第12期农业工程学报V ol.26 No.12
254 2010年12月Transactions of the CSAE Dec. 2010 基于时间序列分析的卡尔曼滤波组合导航算法
周俊1，张鹏1，刘成良2
（1．南京农业大学工学院，南京 210031；2．上海交通大学机械与动力工程学院，上海 200240）
摘 要：GPS广泛用于农业机械导航研究中，其定位误差信号一般存在明显的自相关性，不能满足组合导航中常用的卡尔曼滤波算法观测噪声为高斯白噪声的要求。为此，建立了GPS定位误差AR模型，结合卡尔曼估计结果来预测和修正GPS定位误差，再将修正后的GPS定位信息应用于组合导航中的卡尔曼滤波过程。试验结果表明，无论GPS接收机是在静止还是在运动条件下，处理后的定位误差信号自相关性都明显降低，近似为白噪声；目标路径直线时的最大跟踪误差约为0.15 m，为曲线时，最大跟踪误差约为0.3 m。该方法为低精度GPS应用于农业机械导航提供了可行途径。
关键词：农业机器人，导航，全球定位系统，自回归模型，卡尔曼滤波
doi：10.3969/j.issn.1002-6819.2010.12.043
中图分类号：TP242 文献标志码：A 文章编号：1002-6819(2010)-12-0254-05
周 俊，张 鹏，刘成良. 基于时间序列分析的卡尔曼滤波组合导航算法[J]. 农业工程学报，2010，26(12)：254－258.
Zhou Jun, Zhang Peng, Liu Chengliang. Kalman filtering for integrated navigation based on time series analysis[J]. Transactions of the CSAE, 2010, 26(12): 254－258. (in Chinese with English abstract)
0 引 言
目前农业机械自主导航中使用GPS绝对定位信息比较普遍[1-3]。为了提高导航系统的精度和可靠性，降低成本，一般还需要在Kalman滤波基础上融合其他传感器信息构成某种组合导航形式[4-9]。如果运动载体的运动状态能用线性模型描述且模型误差和传感器测量误差能用高斯白噪声模型化，则 Kalman 滤波将在统计意义上提供融合数据的最优估计；反之，则不能。实际应用中，系统噪声和观测噪声往往是有色噪声，因此造成了Kalman 滤波效果的下降和较大的系统实际误差产生[10]。GPS定位误差就是一个具有自相关性的有色噪声。
本文将采用时间序列分析方法，对农业机械导航过程中的GPS定位误差进行建模，确定GPS定位误差AR 模型参数，消除GPS定位误差信号的内在自相关性，使得GPS定位误差信号接近于白噪声。然后以处理后的GPS定位信息为基础，根据Kalman滤波原理建立农业机械组合导航框架，来提高其自主导航系统性能。
1 GPS定位误差时间序列分析
GPS接收机输出的定位信号在时间上是离散的，信号中存在加性噪声，并随着时间随机变化，当前时刻定位误差信号与该时刻之前的误差信号并不完全独立。可以通过大量的观测数据来分析定位误差特性，建立GPS
收稿日期：2010-02-25 修订日期：2010-08-16
基金项目：国家“863”高技术研究发展计划资助项目（2006AA10Z259，2006AA10A304）
作者简介：周俊（1974—），男，副教授，博士，研究方向：农业机器人、机器视觉与模式识别。南京南京农业大学工学院，210031。
Email: zhoujun@https://www.360docs.net/doc/1115211114.html, 定位误差模型，以进行误差预测和修正。
采用Trimble公司的AgGPS 132接收机，连续采集GPS接收机静止和运动时的定位数据，高斯投影后转换为平面直角坐标系下的位置数据（x,y）。图1a是接收机静止时X坐标方向的连续8 000个采样点构成的误差序列（Y坐标方向有类似的结果，下文同样情况时不再赘述），图1b是对应的定位误差自相关函数曲线。图2a是接收机运动时X坐标的误差序列，图2b是对应的误差自相关函数曲线。可以看出无论是静态还是动态条件下，定位误差序列均具有明显的自相关性。
图1 GPS接收机静止时定位误差
Fig.1 Positioning error with static GPS receiver
第12期 周 俊等：基于时间序列分析的卡尔曼滤波组合导航算法
255
图2 GPS 接收机运动时定位误差
Fig.2 Positioning error with moving GPS receiver
1.1 AR 模型建立
时间序列是随时间改变而随机变化的信号序列，分析的目的就是要找到这种变化规律，建立序列近似的、简化的数学模型，并将其应用于系统动态特性的描述、预测分析和误差补偿等方面[11]。这里采用AR 模型来描述和预测GPS 定位误差信号的变化规律，以提高农业机械组合导航中的定位精度。
对于时间序列{X (t ),t=0,±1,±2,…}而言，AR(p )模型为
1122t t t p t p t x x x x ???ε???=++???++
（1,2,,t p p p n =++???+） （1） 式中，φ1, φ2,…, φp 为自回归系数；p 为AR 模型的阶数；
εt 为均值为0，方差为σ2
的白噪声。
自回归系数φ由AR(p )序列的自协方差函数γ0, γ1,…, γp ,通过Yule-Walker 方程（2）唯一决定。
10
11121022120p p p p p p γγγγ?γγγγ?γγγγ???????????????????????= ??????????????????
????????????
""""""" （2） 白噪声的方差σ2由式（3）求解。
201122()p p σγ?γ?γ?γ=?++???+ （3） 参数估计是在给定阶次的情况下进行的。由于事先无法判断模型的阶次，因此在建模过程中先给定模型的某个阶次，然后按照上述估计方法，估计出AR 模型的参数，得到各阶次模型，最后通过阶数判定准则来确定AR 模型。考虑到实时性要求，应用中在不影响模型精度的前提下尽量选择较低的阶数。 1.2 定位误差预测与修正
当GPS 接收机静止时，由于没有实时性要求，这里选择10阶模型，采用一步预测的方法对误差进行预测
11221010?t t t t x x x x ??????=++???+（10,11,,t n =???）（4）
11221010?()t t t t t t t x x
x x x x ω??????=?=?++???+ （5） 式中，?t x
为误差的一步预测值；ωt 为预测误差和实际误差的差值。应用中将在定位数据的实际观测值上减去预测定位误差，来得到更为准确的GPS 定位信息。
在GPS 接收机静止时，以该位置点多次测量的算术平均值作为真实值，可得到各次测量的定位误差，而后建立定位误差AR 模型。用模型预测值修正实际测量的定位数据，得到新的定位数据测量噪声归一化自相关函数如图3所示。对比图1可以看出，建立的AR 能够准确地描述GPS 定位误差的动态特性，定位数据测量噪声的自相关性显著下降，与白噪声基本一致。
图3 GPS 接收机静止时修正后定位测量噪声自相关函数 Fig.3 Auto-correlation function of modified measurement noise
of positioning data with static GPS receiver
农业机械自主导航过程中，GPS 接收机随农业机械一起运动，每次测量的实际位置点不断变化。由于Kalman 滤波可以实时给出农业机械位置的最优估计，所以把滤波后的位置数据作为对应点的真实值，这样就可在运动中得到GPS 定位误差序列。为了兼顾系统实时性要求，分析中定位误差的AR 模型选为2阶。此时得到的定位误差预测结果以及修正后定位数据观测噪声归一化自相关函数分别如图4a 和4b 所示。由于模型阶数较低以及各测量点真实位置值存在偏差等原因，定位误差的预测精度相较GPS 接收机静止时有所降低，但是仍然可以被模型足够准确地预测出，并且对照图2可见，修正后的定位误差自相关性明显下降，接近于白噪声，这就为其后的Kalman 滤波组合导航提供了高质量的信息源。
农业工程学报 2010年
256
图4 GPS 接收机运动时定位误差预测
Fig.4 Prediction of positioning error with moving GPS receiver
2 GPS 组合导航
鉴于系统可靠性和成本等方面的原因，农业机械导
航过程中一般不会单纯依靠GPS 定位信息，还要融合里程计等其他传感器信息，其中信息融合框架较多使用Kalman 滤波原理。融合中不同源信息之间相互补充，可以提高导航系统精度和可靠性。前文已经指出，在kalman 滤波融合过程中，各信息源观测噪声为白噪声是组合导航系统在统计意义下提供状态数据最优估计的必要条件，而经过AR 模型处理后的GPS 定位误差就可以很好地满足这一点。
Kalman 滤波中，离散化的组合导航系统状态方程和测量方程
(1)()()()
()()()()X k k X k W k Z k H k X k V k Φ+=+=+ （6）
式中，X (k )为k 时刻的n 维状态向量，也是被估计向量；
Z (k )为k 时刻m 维测量向量；
Φ(k )为k 时刻的系统一步转移矩阵；W (k )为k 时刻n 维的系统噪声向量；H (k )为k 时刻的测量矩阵；V (k )为k 时刻的m 维测量噪声。
卡尔曼滤波要求系统噪声和测量噪声是互不相关的零均值白噪声序列，即有
()(()())0T Q k k j
E W k W j k j =?=? ≠?
（7）
()(()())0T R k k j
E V k V j k j =?=? ≠? （8）
(()())0,T E W k V j k j = ? （9）
式中Q (k )，Rk )为系统噪声和观测噪声的协方差矩阵。
设定(x k ,y k )为农业机械k 时刻坐标，即在WGS-84坐标系下高斯投影后的东、北向坐标值，(x k +1,y k +1)为k +1时刻的坐标。v k 为农业机械纵向速度，θk 为航向角，即农业机械纵向与x 轴正向之间的夹角，T 为采样控制周期。不失一般性，假设农业机械在运动过程中横滚角和俯仰角近似为0，转向轮转向过程耗费时间相对于农业机械运动速度而言可忽略不计，同时还假设单位采样时间内农业机械行进速度恒定。根据运动几何关系，不难确定农业机械运动轨迹的递推关系式为
11cos sin k k k k
k k k k
x x Tv y y Tv θθ++=+=+ （10）
考虑计算效率及传感器配置，这里选用线性模型，
则式（6）系统状态方程的状态向量为
()[,,]k k k X k x y v = （11） 则，系统状态转移矩阵
1110cos ()01sin 001k k T k T θΦθ????
??=??????
（12）
系统观测向量为
()[,,]k k k Z k x y v = （13） 则，系统测量矩阵
100()010001H k ??
??=??????
（14）
确定了具体的导航系统状态方程以及观测方程后，
按照卡尔曼滤波原理采用递推形式，即在以前时刻的状态估计基础上，根据当前时刻的量测值递推得到当前时刻的状态最优估计值。与通常农业机械自主导航中不一样的是，这里每次递推时采用的位置测量值是经过定位误差模型预测值修正后的GPS 定位数据。其他方面与通常的卡尔曼滤波组合导航一样，不再赘述。
3 导航试验
在自主研制的轮式农业机器人平台（图5）上进行GPS 组合导航试验，该平台采用CAN 总线分布式控制体系，具有四轮独立驱动和转向能力[12]。在跟踪路径分别为直线和圆弧曲线两种条件下开展试验，以验证本文算法的有效性。试验过程中实时记录了GPS 原始测量数据、AR 模型处理后的定位数据以及卡尔曼滤波结果。
图5 轮式农业移动机器人平台
Fig.5 Prototype of agricultural wheeled mobile robot
图6和图7分别显示的是直线和曲线路径跟踪时的试验情况，其中6a 和7a 是跟踪轨迹图，6b 和7b 是跟踪误差图。自主导航中直接将GPS 接收机接收到的第一个定位信息作为滤波器状态向量的初始值。由于GPS 测量数据存在着较大的漂移和误差[13]，这个初始值实际上与真实值间存在较大偏差。从图6b 和7b 可以看到，导航开始时由于卡尔曼滤波融合的里程计等其他传感器信息还比较少，修正过程对克服GPS 定位误差的作用不大。
第12期周俊等：基于时间序列分析的卡尔曼滤波组合导航算法
257
图6 直线跟踪试验
Fig.6 Navigation experiment of straight path tracking
图7 曲线跟踪试验
Fig.7 Navigation experiment of curve path tracking
由于GPS定位误差不满足高斯白噪声分布，如果直接应用于卡尔曼滤波，那么滤波结果必然将会随着GPS 定位误差一起偏离真实值。这里按前文所述算法，综合卡尔曼滤波结果与GPS定位误差AR模型来共同修正GPS定位数据，对定位误差进行白化处理，然后再提供给卡尔曼滤波器，所以卡尔曼滤波结果很快就与GPS实际测量值分离开，并跟踪上了目标路径的基准线。稳定后直线路径跟踪时的最大误差约为0.15 m，曲线跟踪时的最大误差约为0.3 m。
4 结 论
1）采用时间序列分析与卡尔曼滤波相结合的方法，考察了AgGPS 132接收机在静止和运动两种工况下的定位误差的相关性，建立了定位误差的AR模型，对GPS 定位误差进行了预测修正。试验结果显示，处理后的定位误差自相关性明显降低，近似为白噪声，为农业机械基于卡尔曼滤波的组合导航系统提供了高质量的定位信息源。
2）应用修正后的GPS定位数据，基于卡尔曼滤波原理设计了农业机械GPS/DR组合导航框架。试验结果表明，直线路径跟踪时最大误差约为 0.15 m，曲线跟踪时最大误差约为0.3 m。
[参 考 文 献]
[1] 胡炼，罗锡文，张智刚，等．基于CAN总线的分布式插
秧机导航控制系统设计[J]．农业工程学报，2009，25(12)：
88－92．
Hu Lian, Luo Xiwen, Zhang Zhigang, et al. Design of
distributed navigation control system for rice transplanters
based on controller area network[J]. Transactions of the
CSAE, 2009, 25(12): 88－92. (in Chinese with English
abstract)
[2] 张漫，周建军，籍颖，等．农用车辆自动导航定位方法[J]．农
业工程学报，2009，25(增刊2)：74－77．
Zhang Man, Zhou Jianjun, Ji Ying, et al. Positioning method
for automatic navigation of agricultural vehicle[J].
Transactions of the CSAE, 2009, 25(Supp.2): 74－77. (in
Chinese with English abstract)
[3] 罗锡文，张智刚，赵祚喜，等．东方红X-804拖拉机的
DGPS自动导航控制系统[J]．农业工程学报，2009，25(11)：
139－145．
Luo Xiwen, Zhang Zhigang, Zhao Zuoxi, et al. Design of
DGPS navigation control system for Dongfanghong X-804
tractor[J]. Transactions of the CSAE, 2009, 25(11): 139－
145. (in Chinese with English abstract)
[4] Noguchi N, J F Reid, J Will, et al. Vehicle automation system
based on multi-sensor integration[C]//Annual International
ASAE Meeting, MI, 1998, No.983111.
[5] Bergeijk Van J, Goense D, et al. Digital filter to integrate
global positioning system and dead reckoning[J]. J Agric Eng
Res, 1998, 70(2): 135－143.
[6] Guo L, Y He, Q Zhang, et al. Real-time tractor position
estimation system using a Kalman filter [J]. Transactions of
CSAE, 2002, 18(5): 96－101.
农业工程学报 2010年258
[7] Yoshisada Nagasaka, Hidefumi Saito, Katsuhiko Tamaki, et
al. An autonomous rice transplanter guided by global positioning system and inertial measurement unit[J]. Journal
of Field Robotics 2009, 26(6):537－548.
[8] 张智刚，罗锡文，赵祚喜，等．基于Kalman滤波和纯追
踪模型的农业机械导航控制[J]．农业机械学报，2009，
40(增刊1)：6－12．
Zhang Zhigang, Luo Xiwen ,et al. Trajectory tracking control
method based on Kalman filter and pure pursuit model for agricultural vehicle[J]. Transactions of the CSAM, 2009, 40(Supp.1):6－12. (in Chinese with English abstract)
[9] 籍颖，刘兆祥，刘刚，等．基于Kalman滤波农用车辆导
航定位方法[J]．农业机械学报，2009，40(增刊1)：13－
17．
Ji Ying, Liu Zhaoxiang, Liu Gang, et al. Positions research of
vehicle navigation system based on Kalman filter[J].
Transactions of the CSAM, 2009, 40(Supp.1): 13－17. (in Chinese with English abstract)
[10] 周俊，姬长英．自主车辆导航系统中的多传感器融合技术
[J]．农业机械学报，2002，33(5):113－116．
Zhou Jun, Ji Changying. Multi-sensor fusion in navigation of
autonomous vehicle[J]. Transactions of the CSAM, 2002,
33(5): 113－116. (in Chinese with English abstract)
[11] 曹力，黄圣国．GPS误差的时间序列分析建模研究[J]．计
算机工程与应用，2005，41(35)：213－216．
Cao Li, Huang Shengguo. Studying on GPS errors modeling
by time series analysis[J]. Computer Engineering and Applications, 2005, 41(35): 213－216. (in Chinese with English abstract)
[12] 王友权，周俊，姬长英，等．基于自主导航和全方位转向
的农用机器人设计[J]．农业工程学报，2008，24(7)：
110－113．
Wang Youquan, Zhou Jun, Ji Changying, et al. Design of
agricultural wheeled mobile robot based on autonomous navigation and omnidirectional steering[J]. Transactions of
the CSAE, 2008, 24(7): 110－113. (in Chinese with English
abstract)
[13] 冯斌．AgGPS132定位测量技术研究[J]．农业机械学报，
2002，33(6)：83－85．
Feng Bin. Study on technology of AgGPS132 positioning
measurement[J]. Transactions of the CSAM, 2002, 33(6): 83－85. (in Chinese with English abstract)
Kalman filtering for integrated navigation based on time series analysis
Zhou Jun1, Zhang Peng1, Liu Chengliang2
(1. College of Engineering, Nanjing Agricultural University, Nanjing 210031, China;
2. School of Mechanical Engineering, Shanghai Jiaotong University, Shanghai 200240, China)
Abstract: GPS is applied widely in autonomous navigation of the agricultural machinery. Its positioning error, however, is characterized by autocorrelation, can not satisfy the requirement of Kalman filtering, which is the base of the integrated navigation system of the agricultural machinery. So the characteristic of GPS positioning error was described as AR model with the time series analysis. Then the method to predict and modify the GPS positioning error with AR model and optimal estimation of Kalman filtering was introduced. And the corrected GPS positioning data were applied in the Kalman filtering for the integrated navigation of the agricultural machinery. The experimental results showed that the autocorrelation between neighboring positioning error data was decreased dramatically, and being similar to the white noise, no matter the GPS receiver was static or not. And when the tracked path was straight and curve, the maximum tracking error was about 0.15 m and 0.3 m respectively. This method can provide a viable way to achieve high-accuracy navigation with low-accuracy GPS for the agricultural machinery.
Key words: agricultural robots,navigation, GPS, AR model, Kalman filtering
时间序列分析方法及应用7
青海民族大学 毕业论文 论文题目：时间序列分析方法及应用—以青海省GDP 增长为例研究 学生姓名：学号： 指导教师：职称： 院系：数学与统计学院 专业班级：统计学 二○一五年月日
时间序列分析方法及应用——以青海省GDP增长为例研究 摘要: 人们的一切活动，其根本目的无不在于认识和改造世界，让自己的生活过得更理想。时间序列是指同一空间、不同时间点上某一现象的相同统计指标的不同数值，按时间先后顺序形成的一组动态序列。时间序列分析则是指通过时间序列的历史数据，揭示现象随时间变化的规律，并基于这种规律，对未来此现象做较为有效的延伸及预测。时间序列分析不仅可以从数量上揭示某一现象的发展变化规律或从动态的角度刻画某一现象与其他现象之间的内在数量关系及其变化规律性，达到认识客观世界的目的。而且运用时间序列模型还可以预测和控制现象的未来行为，由于时间序列数据之间的相关关系（即历史数据对未来的发展有一定的影响），修正或重新设计系统以达到利用和改造客观的目的。从统计学的内容来看，统计所研究和处理的是一批有“实际背景”的数据，尽管数据的背景和类型各不相同，但从数据的形成来看，无非是横截面数据和纵截面数据两类。本论文主要研究纵截面数据，它反映的是现象以及现象之间的关系发展变化规律性。在取得一组观测数据之后，首先要判断它的平稳性，通过平稳性检验，可以把时间序列分为平稳序列和非平稳序列两大类。主要采用的统计方法是时间序列分析，主要运用的数学软件为Eviews软件。大学四年在青海省上学，基于此，对青海省的GDP十分关注。本论文关于对1978年到2014年以来的中国的青海省GDP（总共37个数据）进行时间序列分析，并且对未来的三年中国的青海省GDP进行较为有效的预测。希望对青海省的发展有所贡献。 关键词: 青海省GDP 时间序列白噪声预测
卡尔曼滤波算法总结
Kalman_Filter(float Gyro,float Accel) { Angle+=(Gyro - Q_bias) * dt; Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; Pdot[1]= - PP[1][1]; Pdot[2]= - PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; PP[0][1] += Pdot[1] * dt; PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[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 * PP[0][1]; PP[0][0] -= K_0 * t_0; PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; Gyro_x = Gyro - Q_bias; } 首先是卡尔曼滤波的5个方程： -=--+（1）先验估计 X k k AX k k Bu k (|1)(1|1)() -=--+（2）协方差矩阵的预测(|1)(1|1)' P k k AP k k A Q
几种卡尔曼滤波算法理论
自适应卡尔曼滤波 卡尔曼滤波发散的原因 如果卡尔曼滤波是稳定的，随着滤波的推进，卡尔曼滤波估计的精度应该越来越高，滤波误差方差阵也应趋于稳定值或有界值。但在实际应用中，随着量测值数目的增加，由于估计误差的均值和估计误差协方差可能越来越大，使滤波逐渐失去准确估计的作用，这种现象称为卡尔曼滤波发散。 引起滤波器发散的主要原因有两点： （1）描述系统动力学特性的数学模型和噪声估计模型不准确，不能直接真实地反映物理过程，使得模型与获得的量测值不匹配而导致滤波发散。这种由于模型建立过于粗糙或失真所引起的发散称为滤波发散。 （2）由于卡尔曼滤波是递推过程，随着滤波步数的增加，舍入误差将逐渐积累。如果计算机字长不够长，这种积累误差很有可能使估计误差方差阵失去非负定性甚至失去对称性，使滤波增益矩阵逐渐失去合适的加权作用而导致发散。这种由于计算舍入误差所引起的发散称为计算发散。 针对上述卡尔曼滤波发散的原因，目前已经出现了几种有效抑制滤波发散的方法，常用的有衰减记忆滤波、限定记忆滤波、扩充状态滤波、有限下界滤波、平方根滤波、和自适应滤波等。这些方法本质上都是以牺牲滤波器的最优性为代价来抑制滤波发散，也就是说，多数都是次优滤波方法。 自适应滤波 在很多实际系统中，系统过程噪声方差矩阵Q和量测误差方差阵R事先是不知道的，有时甚至连状态转移矩阵 或量测矩阵H也不能确切建立。如果所建立的模型与实际模型不符可能回引起滤波发散。自适应滤波就是这样一种具有抑制滤波发散作用的滤波方法。在滤波过程中，自适应滤波一方面利用量测值修正预测值，同时也对未知的或不确切的系统模型参数和噪声统计参数进行估计修正。自适应滤波的方法很多，包括贝叶斯法、极大似然法、相关法与协方差匹配法，其中最基本也是最重要的是相关法，而相关法可分为输出相关法和新息相关法。
卡尔曼滤波算法(C--C++两种实现代码)
卡尔曼滤波算法实现代码 C++实现代码如下： ============================kalman.h================= =============== // kalman.h: interface for the kalman class. // ////////////////////////////////////////////////////////////////////// #if !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__IN CLUDED_) #define AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C0__INCLU DED_ #if _MSC_VER > 1000 #pragma once #endif// _MSC_VER > 1000 #include
kalman(int x=0,int xv=0,int y=0,int yv=0); //virtual ~kalman(); }; #endif// !defined(AFX_KALMAN_H__ED3D740F_01D2_4616_8B74_8BF57636F2C 0__INCLUDED_) ============================kalman.cpp=============== ================= #include "kalman.h" #include
卡尔曼滤波算法总结
卡尔曼滤波算法总结-标准化文件发布号：（9556-EUATWK-MWUB-WUNN-INNUL-DDQTY-KII
2015.12.12 void Kalman_Filter(float Gyro,float Accel) { Angle+=(Gyro - Q_bias) * dt; Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; Pdot[1]= - PP[1][1]; Pdot[2]= - PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; PP[0][1] += Pdot[1] * dt; PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[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 * PP[0][1]; PP[0][0] -= K_0 * t_0; PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; Gyro_x = Gyro - Q_bias; }
首先是卡尔曼滤波的5个方程： (|1)(1|1)() X k k AX k k Bu k -=--+（1）先验估计 (|1)(1|1)'P k k AP k k A Q -=--+（2）协方差矩阵的预测 ()(|1)'/(|1)')Kg k P k k H HP k k H R =--+（3）计算卡尔曼增益 (|)(|1)()(()(|1))X k k X k k Kg k Z k HX k k =-+--（4）进行修正 5个式子比较抽象，现在直接用实例来说： 一、卡尔曼滤波第一个式子 对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为d dt θω=?，角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移（而且还是变化的），静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。 由此我们得到了当前角度的预测值Angle Angle=Angle+(Gyro - Q_bias) * dt; 其中等号左边Angle 为此时的角度，等号右边Angle 为上一时刻的角度，Gyro 为陀螺仪测的角速度的值，dt 是两次滤波之间的时间间隔，我们的运行周期是4ms 或者6ms 。 同时 Q_bias 也是一个变化的量。 但是就预测来说认为现在的漂移跟上一时刻是相同的，即 Q_bias=Q_bias 将上面两个式子写成矩阵的形式 1_0 1_0 Angle dt Angle dt Q bias Q bia o s Gyr -= + 得到上式，这个式子对应于卡尔曼滤波的第一个式子 (|1)(1|1)() X k k AX k k Bu k -=--+ (|)(|1) P k k I Kg k H P k k =--（（））（5）更新协方差阵
卡尔曼滤波简介及其算法实现代码
卡尔曼滤波简介及其算法实现代码 卡尔曼滤波算法实现代码（C，C＋＋分别实现） 卡尔曼滤波器简介 近来发现有些问题很多人都很感兴趣。所以在这里希望能尽自己能力跟大家讨论一些力所能及的算法。现在先讨论一下卡尔曼滤波器，如果时间和能力允许，我还希望能够写写其他的算法，例如遗传算法，傅立叶变换，数字滤波，神经网络，图像处理等等。 因为这里不能写复杂的数学公式，所以也只能形象的描述。希望如果哪位是这方面的专家，欢迎讨论更正。 卡尔曼滤波器– Kalman Filter 1．什么是卡尔曼滤波器 （What is the Kalman Filter?） 在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！ 卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。如果对这编论文有兴趣，可以到这里的地址下载： https://www.360docs.net/doc/1115211114.html,/~welch/media/pdf/Kalman1960.pdf。 简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。 2．卡尔曼滤波器的介绍 （Introduction to the Kalman Filter） 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就
【经济预测与决策】时间序列分析预测法
经济预测与决策第四章时间序列分析预测法时间序列分析预测法时间序列分析预测法是将预测目标的历史数据按照时间的顺序排列成为时间序列，然后分析它随时间的变化趋势, 外推预测目标的未来值。本章学习目的与要求通过本章的学习，了解时间序列的概念；掌握移动平均法和指数平滑法。本章学习重点和难点重点是移动平均法；难点是指数平滑法。本章内容提示第一节时间序列第二节移动平均法第三节指数平滑法第一节时间序列一、时间序列二、时间序列的影响因素三、时间序列因素的组合形式四、时间序列预测的步骤一、时间序列时间序列是指某种经济统计指标的数值，按时间先后顺序排列起来的数列。时间序列是时间t 的函数，若用Y 表示，则有：Y=Y（t ）。时间序列时间序列按其指标不同，可分为绝对数时间序列、相对数时间序列和平均数时间序列三种。 绝对数时间序列是基本序列。可分为时期序列和时点序列两种。时期序列是指由反映某种社会经济现象在一段时期内发展过程的总量指标所构成的序列。如各个年度的国民生产总值。时点序列是指由反映某种社会经济现象在一定时点上的发展状况的指标所构成的序列。如各个年末的人口总数。 二、时间序列的影响因素一个时间序列是多种因素综合作用的结果。这些因素可以分为四种：1. 长期趋势变动2. 季节变动3. 循环变动4. 不规则变动1. 长期趋势变动长期趋势变动又称倾向变动，它是指伴随着经济的发展，在相当长的持续时间内，单方向的上升、下降或水平变动的因素。它反映了经济现象的主要 变动趋势。长期趋势变动是时间t 的函数，它反映了不可逆转的倾向的变动。长期趋势变动通常用T表示，T=T( t )。2.循环变动循环变动是围绕于
时间序列分析——最经典的
【时间简“识”】 说明：本文摘自于经管之家(原人大经济论坛) 作者：胖胖小龟宝。原版请到经管之家(原人大经济论坛) 查看。 1.带你看看时间序列的简史 现在前面的话—— 时间序列作为一门统计学，经济学相结合的学科，在我们论坛，特别是五区计量经济学中是热门讨论话题。本月楼主推出新的系列专题——时间简“识”，旨在对时间序列方面进行知识扫盲（扫盲，仅仅扫盲而已……），同时也想借此吸引一些专业人士能够协助讨论和帮助大家解疑答惑。 在统计学的必修课里，时间序列估计是遭吐槽的重点科目了，其理论性强，虽然应用领域十分广泛，但往往在实际操作中会遇到很多“令人发指”的问题。所以本帖就从基础开始，为大家絮叨絮叨那些关于“时间”的故事！ Long long ago，有多long估计大概7000年前吧，古埃及人把尼罗河涨落的情况逐天记录下来，这一记录也就被我们称作所谓的时间序列。记录这个河流涨落有什么意义当时的人们并不是随手一记，而是对这个时间序列进行了长期的观察。结果，他们发现尼罗河的涨落非常有规律。掌握了尼罗河泛滥的规律，这帮助了古埃及对农耕和居所有了规划，使农业迅速发展，从而创建了埃及灿烂的史前文明。
好~~从上面那个故事我们看到了 1、时间序列的定义——按照时间的顺序把随机事件变化发展的过程记录下来就构成了一个时间序列。 2、时间序列分析的定义——对时间序列进行观察、研究，找寻它变化发展的规律，预测它将来的走势就是时间序列分析。 既然有了序列，那怎么拿来分析呢 时间序列分析方法分为描述性时序分析和统计时序分析。 1、描述性时序分析——通过直观的数据比较或绘图观测，寻找序列中蕴含的发展规律，这种分析方法就称为描述性时序分析 描述性时序分析方法具有操作简单、直观有效的特点，它通常是人们进行统计时序分析的第一步。 2、统计时序分析 （1）频域分析方法 原理：假设任何一种无趋势的时间序列都可以分解成若干不同频率的周期波动 发展过程： 1）早期的频域分析方法借助富里埃分析从频率的角度揭示时间序列的规律 2）后来借助了傅里叶变换，用正弦、余弦项之和来逼近某个函数 3）20世纪60年代，引入最大熵谱估计理论，进入现代谱分析阶段 特点：非常有用的动态数据分析方法，但是由于分析方法复杂，结果抽象，有一定的使用局限性 （2）时域分析方法
Kalman滤波算法
Kalman 滤波算法 姓名：刘金强 专业：控制理论与控制工程 学号：2007255 ◆实验目的： （1）、掌握klman 滤波实现的原理和方法 （2）、掌握状态向量预测公式的实现过程 （3）、了解Riccati 差分方程实现的过程和新息的基本性质和过程的计算 ◆实验要求： 问题： F=[a1,a2,a3],其中a1=[1.0 0 0]的转置，a2=[0.3 1.0 0]的转置，a3=[0.1 0.2 0.4]的转置，x(0)=[3,-1,2]的转置；C=[b1,b2,b3],其中b1=[0.3 0.5]的转置，b2=[1,0.4]的转置，b3=[0.8 -0.7]的转置；V1(n)=[0 0 n1(n)sin(0.1n)]的转置，V2(n)=[n2(n) n3(n)];n1(n)为均值为零，方差为1的均匀分布白噪声;n2(n),n3(n)为均值为0，方差为0.1的均匀分布白噪声,n1(n),n2(n),n3(n)相互独立，试用卡尔曼滤波器算法估计x^(n). ◆实验原理： 初始条件： 1?(1)x =E{x(1)} K(1,0)=E{[x(1)- (1)x ][x(1)- (1)H x ]},其中(1)x =E{x(1)} 输入观测向量过程： 观测向量序列={y(1),…………y(n)} 已知参数： 状态转移矩阵F(n+1,n) 观测矩阵C(n) 过程噪声向量的相关矩阵1()Q n 观测噪声向量的相关矩阵2()Q n 计算：n=1,2,3,………………. G(n)=F(n+1,n)K(n,n+1) ()H C n 12[()(,1)()()]H C n K n n C n Q n --+ Kalman 滤波器是一种线性的离散时间有限维系统。Kalman 滤波器的估计性能是：它使滤波后的状态估计误差的相关矩阵P(n)的迹最小化。这意味着，kalman 滤波器是状态向量x(n)的线性最小方差估计。 ◆实验结果： ◆程序代码： （1）主程序
季节性时间序列分析方法
季节性时间序列分析方 法 LG GROUP system office room 【LGA16H-LGYY-LGUA8Q8-LGA162】
第七章季节性时间序列分析方法 由于季节性时间序列在经济生活中大量存在，故将季节时间序列从非平稳序列中抽出来，单独作为一章加以研究，具有较强的现实意义。本章共分四节：简单随机时间序列模型、乘积季节模型、季节型时间序列模型的建立、季节调整方法X-11程序。 本章的学习重点是季节模型的一般形式和建模。 §1 简单随机时序模型 在许多实际问题中，经济时间序列的变化包含很多明显的周期性规律。比如：建筑施工在冬季的月份当中将减少，旅游人数将在夏季达到高峰，等等，这种规律是由于季节性（seasonality）变化或周期性变化所引起的。对于这各时间数列我们可以说，变量同它上一年同一月（季度，周等）的值的关系可能比它同前一月的值的相关更密切。 一、季节性时间序列 1．含义：在一个序列中，若经过S个时间间隔后呈现出相似性，我们说该序列具有以S为周期的周期性特性。具有周期特性的序列就称为季节性时间序列，这里S为周期长度。 注：①在经济领域中，季节性的数据几乎无处不在，在许多场合，我们往往可以从直观的背景及物理变化规律得知季节性的周期，如季度数据（周期为4）、月度数据（周期为12）、周数据（周期为7）；②有的时间序列也可能包含长度不同的若干种周期，如客运量数据（S=12，S=7） 2．处理办法： （1）建立组合模型； （1）将原序列分解成S个子序列（Buys-Ballot 1847）
对于这样每一个子序列都可以给它拟合ARIMA 模型，同时认为各个序列之间是相互独立的。但是这种做法不可取，原因有二：（1）S 个子序列事实上并不相互独立，硬性划分这样的子序列不能反映序列{}t x 的总体特征；（2）子序列的划分要求原序列的样本足够大。 启发意义：如果把每一时刻的观察值与上年同期相应的观察值相减，是否能将原序列的周期性变化消除（ 或实现平稳化），在经济上，就是考查与前期相比的净增值，用数学语言来描述就是定义季节差分算子。 定义：季节差分可以表示为S t t t S t S t X X X B X W --=-=?=)1(。 二、 随机季节模型 1．含义：随机季节模型，是对季节性随机序列中不同周期的同一周期点之间的相关关系的一种拟合。 AR （1）：t t S t S t t e W B e W W =-?+=-)1(11??，可以还原为：t t S S e X B =?-)1(1?。 MA （1）：t S t S t t t e B W e e W )1(11θθ-=?-=-，可以还原为：t S t S e B X )1(1θ-=?。 2．形式：广而言之，季节型模型的ARMA 表达形式为 t S t S e B V W B U )()(= （1） 这里，?? ? ??----=----=?=qS q S S S pS P S S S t d S t B V B V B V B V B U B U B U B U X W 2212211)(1)()(平稳。 注：（1）残差t e 的内容；（2）残差t e 的性质。 §2 乘积季节模型 一、 乘积季节模型的一般形式 由于t e 不独立，不妨设),,(~m d n ARIMA e t ，则有
卡尔曼(kalman)滤波算法特点及其应用
Kalman滤波算法的特点： （1）由于Kalman滤波算法将被估计的信号看作在白噪声作用下一个随机线性系统的输出，并且其输入/输出关系是由状态方程和输出方程在时间域内给出的，因此这种滤波方法不仅适用于平稳随机过程的滤波，而且特别适用于非平稳或平稳马尔可夫序列或高斯-马尔可夫序列的滤波，所以其应用范围是十分广泛的。 （2）Kalman滤波算法是一种时间域滤波方法，采用状态空间描述系统。系统的过程噪声和量测噪声并不是需要滤除的对象，它们的统计特征正是估计过程中需要利用的信息，而被估计量和观测量在不同时刻的一、二阶矩却是不必要知道的。 （3）由于Kalman滤波的基本方程是时间域内的递推形式，其计算过程是一个不断地“预测-修正”的过程，在求解时不要求存储大量数据，并且一旦观测到了新的数据，随即可以算的新的滤波值，因此这种滤波方法非常适合于实时处理、计算机实现。 （4）由于滤波器的增益矩阵与观测无关，因此它可预先离线算出，从而可以减少实时在线计算量。在求滤波器增益矩阵时，要求一个矩阵的逆，它的阶数只取决于观测方程的维数，而该维数通常很小，这样，求逆运算是比较方便的。另外，在求解滤波器增益的过程中，随时可以算出滤波器的精度指标P，其对角线上的元素就是滤波误差向量各分量的方差。 Kalman滤波的应用领域 一般地，只要跟时间序列和高斯白噪声有关或者能建立类似的模型的系统，都可以利用Kalman滤波来处理噪声问题，都可以用其来预测、滤波。Kalman滤波主要应用领域有以下几个方面。 （1）导航制导、目标定位和跟踪领域。 （2）通信与信号处理、数字图像处理、语音信号处理。 （3）天气预报、地震预报。 （4）地质勘探、矿物开采。 （5）故障诊断、检测。 （6）证券股票市场预测。 具体事例： （1）Kalman滤波在温度测量中的应用； （2）Kalman滤波在自由落体运动目标跟踪中的应用； （3）Kalman滤波在船舶GPS导航定位系统中的应用； （4）Kalman滤波在石油地震勘探中的应用； （5）Kalman滤波在视频图像目标跟踪中的应用；
时间序列分析方法第章预测
第四章 预 测 在本章当中我们讨论预测的一般概念和方法，然后分析利用),(q p ARMA 模型进行预测的问题。 §4.1 预期原理 利用各种条件对某个变量下一个时点或者时间阶段内取值的判断是预测的重要情形。为此，需要了解如何确定预测值和度量预测的精度。 4.1.1 基于条件预期的预测 假设我们可以观察到一组随机变量t X 的样本值，然后利用这些数据预测随机变量1+t Y 的值。特别地，一个最为简单的情形就是利用t Y 的前m 个样本值预测1+t Y ，此时t X 可以描述为： 假设*|1t t Y +表示根据t X 对于1+t Y 做出的预测。那么如何度量预测效果呢？通常情况下，我们利用损失函数来度量预测效果的优劣。假设预测值与真实值之间的偏离作为损失，则简单的二次损失函数可以表示为(该度量也称为预测的均方误差)： 定理4.1 使得预测均方误差达到最小的预测是给定t X 时，对1 +t Y 的条件数学期望，即： 证明：假设基于t X 对1+t Y 的任意预测值为： 则此预测的均方误差为： 对上式均方误差进行分解，可以得到： 其中交叉项的数学期望为(利用数学期望的叠代法则)： 因此均方误差为： 为了使得均方误差达到最小，则有： 此时最优预测的均方误差为： 211*|1)]|([)(t t t t t X Y E Y E Y MSE +++-= End 我们以后经常使用条件数学期望作为随机变量的预测值。 4.1.2 基于线性投影的预测 由于上述条件数学期望比较难以确定，因此将预测函数的范围限制在线性函数当中，我们考虑下述线性预测： 如此预测的选取是所有预测变量的线性组合，预测的优劣则体现在系数向量的选择上。 定义4.1 如果我们可以求出一个系数向量值α，使得预测误差)(1t t X Y α'-+与t X 不相关： 则称预测t X α'为1+t Y 基于t X 的线性投影。 定理4.2 在所有线性预测当中，线性投影预测具有最小的均方误差。
时间序列分析法原理及步骤
时间序列分析法原理及步骤 ----目标变量随决策变量随时间序列变化系统 一、认识时间序列变动特征 认识时间序列所具有的变动特征, 以便在系统预测时选择采用不同的方法 1》随机性:均匀分布、无规则分布,可能符合某统计分布(用因变量的散点图和直方图及其包含的正态分布检验随机性, 大多服从正态分布 2》平稳性:样本序列的自相关函数在某一固定水平线附近摆动, 即方差和数学期望稳定为常数 识别序列特征可利用函数 ACF :其中是的 k 阶自 协方差,且 平稳过程的自相关系数和偏自相关系数都会以某种方式衰减趋于 0, 前者测度当前序列与先前序列之间简单和常规的相关程度, 后者是在控制其它先前序列的影响后,测度当前序列与某一先前序列之间的相关程度。实际上, 预测模型大都难以满足这些条件, 现实的经济、金融、商业等序列都是非稳定的,但通过数据处理可以变换为平稳的。 二、选择模型形式和参数检验 1》自回归 AR(p模型
模型意义仅通过时间序列变量的自身历史观测值来反映有关因素对预测目标的影响和作用,不受模型变量互相独立的假设条件约束,所构成的模型可以消除普通回归预测方法中由于自变量选择、多重共线性的比你更造成的困难用 PACF 函数判别 (从 p 阶开始的所有偏自相关系数均为 0 2》移动平均 MA(q模型 识别条件
平稳时间序列的偏相关系数和自相关系数均不截尾,但较快收敛到 0, 则该时间序列可能是 ARMA(p,q模型。实际问题中,多数要用此模型。因此建模解模的主要工作时求解 p,q 和φ、θ的值,检验和的值。 模型阶数 实际应用中 p,q 一般不超过 2. 3》自回归综合移动平均 ARIMA(p,d,q模型 模型含义 模型形式类似 ARMA(p,q模型, 但数据必须经过特殊处理。特别当线性时间序列非平稳时,不能直接利用 ARMA(p,q模型,但可以利用有限阶差分使非平稳时间序列平稳化,实际应用中 d (差分次数一般不超过 2. 模型识别 平稳时间序列的偏相关系数和自相关系数均不截尾,且缓慢衰减收敛,则该时间序列可能是 ARIMA(p,d,q模型。若时间序列存在周期性波动, 则可按时间周期进
卡尔曼滤波算法及MATLAB实现
基于matlab的卡尔曼信号滤波设计 卡尔曼滤波的基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。 语音信号在较长时间内是非平稳的，但在较短的时间内的一阶统计量和二阶统计量近似为常量，因此语音信号在相对较短的时间内可以看成白噪声激励以线性时不变系统得到的稳态输出。假定语音信号可看成由一AR模型产生： 时间更新方程： 测量更新方程： K(t)为卡尔曼增益，其计算公式为： 其中 、分别为过程模型噪声协方差和测量模型噪声协方差，测量协方差可以通过观测得到， 则较难确定，在本实验中则通过与两者比较得到。 由于语音信号短时平稳，因此在进行卡尔曼滤波之前对信号进行分帧加窗操作，在滤波之后对处理得到的信号进行合帧，这里选取帧长为256，而帧重叠个数为128； 下图为原声音信号与加噪声后的信号以及声音信号与经卡尔曼滤波处理后的信号：
原声音信号与加噪声后的信号 原声音信号与经卡尔曼滤波处理后的信号 MATLAB程序实现如下： %%%%%%%%%%%%%%%%%基于LPC全极点模型的最大后验概率估计法,采用卡尔曼滤波%%%%%%%%%%%%%% clear; clc; %%%%%%%%%%%%%%%%%%%%%%%%%%%加载声音数据%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% loadvoice.mat y=m1(2,:); x=y+0.08*randn(1,length(y)); %%%%%%%%%%%%%%%原声音信号和加噪声后的信号%%%%%%%%%%%%%%% figure(1); subplot(211);plot(m1(1,:),m1(2,:));xlabel('时间');ylabel('幅度');title('原声音信号'); subplot(212);plot(m1(1,:),x);xlabel('时间');ylabel('幅度');title('加噪声后的信号');
卡尔曼滤波算法总结
2015.12.12 void Kalman_Filter(float Gyro,float Accel) { Angle+=(Gyro - Q_bias) * dt; Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; Pdot[1]= - PP[1][1]; Pdot[2]= - PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; PP[0][1] += Pdot[1] * dt; PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; PCt_0 = C_0 * PP[0][0]; PCt_1 = C_0 * PP[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 * PP[0][1]; PP[0][0] -= K_0 * t_0; PP[0][1] -= K_0 * t_1; PP[1][0] -= K_1 * t_0; PP[1][1] -= K_1 * t_1; Angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; Gyro_x = Gyro - Q_bias; }
首先是卡尔曼滤波的5个方程： (|1)(1|1)()X k k AX k k Bu k -=--+（1）先验估计 (|1)(1|1)'P k k AP k k A Q -=--+（2）协方差矩阵的预测 ()(|1)'/(|1)') Kg k P k k H HP k k H R =--+（3）计算卡尔曼增益 (|)(|1)()(()(|1)) X k k X k k Kg k Z k HX k k =-+--（4）进行修正 5个式子比较抽象，现在直接用实例来说： 一、卡尔曼滤波第一个式子 对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为d dt θω=?，角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移（而且还是变化的），静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。 由此我们得到了当前角度的预测值Angle Angle=Angle+(Gyro - Q_bias) * dt; 其中等号左边Angle 为此时的角度，等号右边Angle 为上一时刻的角度，Gyro 为陀螺仪测的角速度的值，dt 是两次滤波之间的时间间隔，我们的运行周期是4ms 或者6ms 。 同时 Q_bias 也是一个变化的量。 但是就预测来说认为现在的漂移跟上一时刻是相同的，即 Q_bias=Q_bias 将上面两个式子写成矩阵的形式 1_01_0 Angle dt Angle dt Q bias Q bia o s Gyr -=+ 得到上式，这个式子对应于卡尔曼滤波的第一个式子 (|1)(1|1)()X k k AX k k Bu k -=--+ ()|1X k k -为2维列向量 _Angle Q bias ，A 为2维方阵101dt -，()|-11X k k -为2维列向量_Angle Q bias ，B 为2维列向量0dt ， ()u k 为Gyro (|)(|1) P k k I Kg k H P k k =--（（））（5）更新协方差阵
时间序列分析方法第章谱分析完整版
时间序列分析方法第章 谱分析 HEN system office room 【HEN16H-HENS2AHENS8Q8-HENH1688】
第六章 谱分析 Spectral Analysis 到目前为止，t 时刻变量t Y 的数值一般都表示成为一系列随机扰动的函数形式，一般的模型形式为： 我们研究的重点在于，这个结构对不同时点t 和τ上的变量t Y 和τ Y 的协方差具有什么样的启示。这种方法被称为在时间域(time domain)上分析时间序列+∞∞-}{t Y 的性质。 在本章中，我们讨论如何利用型如)cos(t ω和)sin(t ω的周期函数的加权组合来描述时间序列t Y 数值的方法，这里ω表示特定的频率，表示形式为： 上述分析的目的在于判断不同频率的周期在解释时间序列+∞∞ -}{t Y 性质时所发挥的重要程度如何。如此方法被称为频域分析(frequency domain analysis)或者谱分析(spectral analysis)。我们将要看到，时域分析和频域分析之间不是相互排斥的，任何协方差平稳过程既有时域表示，也有频域表示，由一种表示可以描述的任何数据性质，都可以利用另一种表示来加以体现。对某些性质来说，时域表示可能简单一些；而对另外一些性质，可能频域表示更为简单。 § 母体谱 我们首先介绍母体谱，然后讨论它的性质。 6.1.1 母体谱及性质 假设+∞∞-}{t Y 是一个具有均值μ的协方差平稳过程，第j 个自协方差为： 假设这些自协方差函数是绝对可加的，则自协方差生成函数为： 这里z 表示复变量。将上述函数除以π2，并将复数z 表示成为指数虚数形式)ex p(ωi z -=，1-=i ，则得到的结果(表达式)称为变量Y 的母体谱： 注意到谱是ω的函数：给定任何特定的ω值和自协方差j γ的序列+∞∞-}{j γ，原则上都可以计算)(ωY s 的数值。 利用De Moivre 定理，我们可以将j i e ω-表示成为： 因此，谱函数可以等价地表示成为： 注意到对于协方差平稳过程而言，有：j j -=γγ，因此上述谱函数化简为： 利用三角函数的奇偶性，可以得到： 假设自协方差序列+∞∞-}{j γ是绝对可加的，则可以证明上述谱函数
第六章时间序列分析
第六章时间序列分析 重点： 1、增长量分析、发展水平及增长量 2、增长率分析、发展速度及增长速度 3、时间数列影响因素、长期趋势分析方法 难点： 1、增长量与增长速度 2、长期趋势与季节变动分析 第一节时间序列的分析指标 知识点一：时间序列的含义 时间序列是指经济现象按时间顺序排列形成的序列。这种数据称为时间序列数据。 时间序列分析就是根据这样的数列分析经济现象的发展规律，进而预测其未来水平。 时间数列是一种统计数列，它是将反映某一现象的统计指标在不同时间上的数值按时间先后顺序排列所形成的数列。表现了现象在时间上的动态变化，故又称为动态数列。 一个完整的时间数列包含两个基本要素： 一是被研究现象或指标所属的时间； 另一个是该现象或指标在此时间坐标下的指标值。 同一时间数列中，通常要求各指标值的时间单位和时间间隔相等，如无法保证相等，在计算某些指标时就涉及到“权”的概念。 研究时间数列的意义：了解与预测。 [例题·单选题]下列数列中哪一个属于时间数列（）. a.学生按学习成绩分组形成的数列 b.一个月内每天某一固定时点记录的气温按度数高低排列形成的序列 c.工业企业按产值高低形成的数列 d.降水量按时间先后顺序排列形成的数列 答案：d 解析：时间序列是一种统计数列，它是将反映某一现象的统计指标在不同时间上的数值按时间先后顺序排列所形成的数列，表现了现象在时间上的动态变化。 知识点二：增长量分析（水平分析）
一.发展水平 发展水平是指客观现象在一定时期内（或时点上）发展所达到的规模、水平,一般用y t （t=1,2,3，…,n) 。 在绝对数时间数列中，发展水平就是绝对数； 在相对数时间数列中，发展水平就是相对数或平均数。 几个概念：期初水平y 0，期末水平y t ，期间水平(y 1 ,y 2 ,….y n-1 )； 报告期水平(研究时期水平)，基期水平（作为对比基础的水平）。 二.增长量 增长量是报告期发展水平与基期发展水平之差，增长量的指标数值可正可负，它反映的是报告期相对基期增加或减少的绝对数量，用公式表示为： 增长量＝报告期水平－基期水平 根据基期的不同确定方法，增长量可分为逐期增长量和累计增长量。 1.逐期增长量：是报告期水平与前一期水平之差，用公式表示为： △ = y n - y n-1 （i=1,2,…,n） 2.累计增长量：是报告期水平与某一固定时期水平（通常是时间序列最初水平）之差，用公式表示为： △ = y n - y （i=1,2,…,n）（i=1,2,…,n） 二者关系：逐期增长量之和=累计增长量 3.平均增长量 平均增长量是时间序列中的逐期增长量的序时平均数，它表明现象在一定时段内平均每期增加（减少）的数量。 一般用累计增长量除以增长的时期数目计算。 （y n - y ）/n [例题·单选题]某社会经济现象在一定时期内平均每期增长的绝对数量是（）。 a．逐期增长量 b．累计增长量 c．平均增长量 d．增长速度 答案：c 解析：平均每期增长的绝对数量是平均增长量。 知识点三：增长率分析（速度分析） 一.发展速度
卡尔曼滤波算法
卡尔曼滤波器的介绍 为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。 在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。 假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下 一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位） 。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声，也就是这些偏差跟前后时间是没有关系的而且符合高斯分配。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。 好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。 假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻
的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25 度，同时该值的偏差是4度。 由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg^2=5^2/(5^2+4^2)所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。 现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现 在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的