【TRIO-Basic从入门到精通教程九】卡尔曼滤波器算法编程与测试

作者：Gmotion 发布时间：2017-11-10 更新时间：2023-12-30 点击数：

亲爱的朋友们，好久没有见面了！今天给大家带来实质性的算法，卡尔曼滤波器！很多朋友肯定又要问，卡尔曼到底是什么东东，具体有什么用处？这就是今天实验的目标！

解释原理，我们摘抄一段百科里面的描述：

//=====================================================================================================================================

卡尔曼滤波（Kalman filtering)一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA 埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表。数据滤波是去除噪声还原真实数据的一种数据处理技术, Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态. 由于, 它便于计算机编程实现, 并能够对现场采集的数据进行实时的更新和处理, Kalman滤波是目前应用最为广泛的滤波方法, 在通信, 导航, 制导与控制等多领域得到了较好的应用 . //===================================================================================================================================

用通俗的话语讲：通入的原始数据抖动大，难于直接使用。相信很多朋友都是用过采用平均值的方法去除抖动，让输入数据平稳，但平均值会把误差噪声也引入，影响结果并且严重滞后输入反应。

是不是很神奇，我们接下来进入重点：

//===================================================================================================================================

DIM srcdata,resrcdata,outdata AS FLOAT
'====================================================================
DIM measurenoise_r,processniose_q AS FLOAT
measurenoise_r=10
processniose_q=0.5

DIM calall_data,calindex AS INTEGER
DIM state_x,state_k,state_noise AS FLOAT
calall_data=200
calindex=0

DIM serdata_tadr,noisedata_tadr,fliterdata_tadr,tablebuf_tadr AS INTEGER
serdata_tadr=0
noisedata_tadr=1000
fliterdata_tadr=2000
tablebuf_tadr=3000

TABLE(serdata_tadr,0)
TABLE(noisedata_tadr,0)
TABLE(fliterdata_tadr,0)
FOR calindex=1 TO calall_data
    srcdata=100*SIN(PI*calindex/200)
    TABLE(serdata_tadr+calindex,srcdata)
    resrcdata=srcdata+(RND(100)-50)*0.05
    TABLE(noisedata_tadr+calindex,resrcdata)
    WA(2)
    '===================================================
    'FUNCTION kalmanfilter(serdata AS FLOAT,man_r AS FLOAT,proc_q AS FLOAT,tableadr AS INTEGER) AS FLOAT
    outdata=kalmanfilter(resrcdata,measurenoise_r,processniose_q,tablebuf_tadr)
    '===================================================
    TABLE(fliterdata_tadr+calindex,outdata)
NEXT calindex

//===================================================================================================================================
首先我们画了一个正玄曲线
srcdata=100*SIN(PI*calindex/200)