滤波算法学习与讨论

最近要用到滤波算法,先前转载了个卡尔曼的c,但是不知道效果和怎么使用,

听说研究算法都要用到matlab,可惜不会...

就用aar+echarts来表示下波形好了.

blob.png

上图是我在aar里直接调用C语言的那个卡尔曼产生的波形.

感觉不忍直视,算法肯定调用有错误,这个aar调用c函数看来应该不像想象中那样用....

var data1string ,data2string,data3string = "{data:[","{data:[","{data:[";	

//aardio调用C语言函数
var code = /****
#include <stdio.h>
#include <stdlib.h> 

long KalmanFilter(long ResrcData)
{
     /*-------------------------------------------------------------------------------------------------------------*/
     /*
             Q:过程噪声,Q增大,动态响应变快,收敛稳定性变坏
             R:测量噪声,R增大,动态响应变慢,收敛稳定性变好
     */
     /*-------------------------------------------------------------------------------------------------------------*/
     static long R = (int32_t)(128*1024);
     static long Q = (int32_t)4;
     static long Counter1 ;
     static long Counter2;
     static long x_last;
        static long p_last;   // 应赋初始估计值
     long x_mid;
     long x_now;
     long p_mid ;
     long p_now;
 
     ResrcData *= 1024;
     x_now = ResrcData - x_last;
     if(x_now < 0)
     {
         x_now *= -1; // 取绝对值
     }
     if(x_now >= 32*1024)   // 如果测量值连续比估计值大或小 相信测量值,加速迭代
     {
         Counter1++;
         Counter2 = 0;
         if(Counter1 > 10)
         {
             R = 512;;
             Q = 128;
         }
     }
     else                 // 数据比较稳定,加强滤波 
     {
         Counter1 = 0;
         Counter2++;
         if(Counter2 > 10)  
         {
             R = (int)(128*1024);
             Q = (int)4;
         }
     }
     x_mid = x_last;   // x_last=x(k-1|k-1),x_mid=x(k|k-1)
     p_mid = p_last + Q; // p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
//    kg = p_mid/(p_mid + R); //kg为kalman filter,R为噪声
//    x_now = x_mid+kg*(ResrcData - x_mid);// 估计出的最优值
     x_now = x_mid + (p_mid*(ResrcData - x_mid))/(p_mid + R);
//    p_now = (1 - kg)*p_mid; // 最优值对应的covariance
     p_now = p_mid - p_mid*p_mid/(p_mid + R); // 最优值对应的covariance
     p_last = p_now;  // 更新covariance值
     x_last = x_now;  // 更新系统状态值
     x_now /= 1024;
     if((x_now > 4096)||( x_now < 0))
     {
         x_last = ResrcData;
         p_now = ResrcData;
         x_now = ResrcData/1024;
     }
     return (int)x_now;
}

****/
mainForm.button2.oncommand = function(id,event){
//mainForm.msgbox( mainForm.button2.text );

import console;
import tcc;  
console.open()

var vm = tcc( ); //创建TCC编译器 
vm.compile(code); //编译C源码

//调用C函数
var data1,data2 = 0,0;
for(i=1;179;1){
data1 = math.round(math.sin(math.rad(i))*500);
data2 = math.round(math.sin(math.rad(i))*500) + math.random(-5, 5);
var ret = vm.KalmanFilter(data2);
console.log( "C函数返回值2",data1,data2, ret )

data1string = data1string ++ data1 ++ ",";
data2string = data2string ++ data2 ++ ",";
data3string = data3string ++ ret ++ ",";
//console.more(50)
}




}

mainForm.button3.oncommand = function(id,event){
//mainForm.msgbox( mainForm.button3.text );


var datastr;
var datalist_start = "var option = { series:["
var datalist_end = "] }; myChart.setOption(option);";

datastr = datalist_start ++ data1string ++ "]},"++ data2string ++ "]},"++ data3string ++ "]}," ++ datalist_end;
wbKitView.doScript(datastr);
}

下面还是老老实实把C转换成aar语句去..

4 个评论

转换之后


/*
            Q:过程噪声,Q增大,动态响应变快,收敛稳定性变坏
            R:测量噪声,R增大,动态响应变慢,收敛稳定性变好
    */
var aar_R = 1024*10//128*1024;
var aar_Q = 250//4; //让其收敛快点
var aar_Counter1 = 0 ;    //aardio中必须赋值,要不然就是null
var aar_Counter2 = 0 ;
var aar_x_last = 0 ;
var aar_p_last = 0;   // 应赋初始估计值
var aar_x_mid = 0;
var aar_x_now = 0;
var aar_p_mid = 0 ;
var aar_p_now = 100;

aar_KalmanFilter = function( ResrcData){


 
     ResrcData *= 1024;
     aar_x_now = ResrcData - aar_x_last;
     if(aar_x_now < 0)
     {
         aar_x_now = math.abs(aar_x_now); // 取绝对值
     }
     //下面的动态判断更改R和Q被我注释掉了,是一楼中的图形就是这个导致的..变成了阶梯状
/*
     if(aar_x_now >= 32*1024)   // 如果测量值连续比估计值大或小 相信测量值,加速迭代
     {
         aar_Counter1++;
         aar_Counter2 = 0;
         if(aar_Counter1 > 10)
         {
             aar_R = 512;;
             aar_Q = 128;
         }
     }
     else                 // 数据比较稳定,加强滤波 
     {
         aar_Counter1 = 0;
         aar_Counter2++;
         if(aar_Counter2 > 10)  
         {
             aar_R = 128*1024;
             aar_Q = 4;
         }
     }
*/
     aar_x_mid = aar_x_last;   // x_last=x(k-1|k-1),x_mid=x(k|k-1)
     aar_p_mid = aar_p_last + aar_Q; // p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
//    kg = p_mid/(p_mid + R); //kg为kalman filter,R为噪声
//    x_now = x_mid+kg*(ResrcData - x_mid);// 估计出的最优值
     aar_x_now = aar_x_mid + (aar_p_mid*(ResrcData - aar_x_mid))/(aar_p_mid + aar_R);
//    p_now = (1 - kg)*p_mid; // 最优值对应的covariance
     aar_p_now = aar_p_mid - aar_p_mid*aar_p_mid/(aar_p_mid + aar_R); // 最优值对应的covariance
     aar_p_last = aar_p_now;  // 更新covariance值
     aar_x_last = aar_x_now;  // 更新系统状态值
     aar_x_now /= 1024;
     if((aar_x_now > 4096)||( aar_x_now < 0))
     {
         aar_x_last = ResrcData;
         aar_p_now = ResrcData;
         aar_x_now = ResrcData/1024;
     }
     return aar_x_now;

}

同样的,把C的那个还保留着,在程序中增加了aar的卡尔曼滤波,这样也可以对比C在aar中执行是不是和直接在aar写的函数一致..

将C写的卡尔曼R和Q分别改为10*1024和4 ,将aar写的滤波RQ分别改为10*1024和250

运行后,得出以下波形

blob.png

可以看出:

C函数在aar中能够很好的被支持调用,滤波中的Q值影响的确实是收敛的速度,上图汇总蓝色线是C中的Q是4, 棕色线是aar中的Q是250, 棕色的明显比蓝色的收敛的速度快

再次将两种方式的RQ中的Q都设为250的收敛速度,将aar中的R值改为1024,而C中的R值改为1024*10,运行后如下图

blob.png

aar的收敛速度明显比C快,但是稳定度却没有C的好了.

说明R和Q要搭配好才能发挥最大优势,无论盲目的增加和减少任意一个都不能达到最佳效果

在C程序中再次加入滑动滤波算法:

long value_buff[10]={0};                     //N相当于选定一个窗口大小,对窗口数据做平均!
char i=0;
char j=0;
long hdfilter(long hddata)
{
     char count;
     long sum=0;
     value_buff[j++] = hddata;
     i++;
     if(j>=10){
      j = 0;
     }
     if(i>=10)
     {
      i = 9;
      for(count=0;count<10;count++)
        sum += value_buff[count];
return (long)(sum/10); 
     }else {
      for(count=0;count<i;count++)
        sum += value_buff[count];
return (long)(sum/i);
     }

}

调用滑动滤波,增加曲线到图中,动态效果图如下:

GIF.gif

可以看到,滑动滤波效果和卡尔曼滤波效果差不多,某些突变比较厉害的地方卡尔曼更胜一筹.

准备弄个C语言产生高斯白噪声,这样模拟AD噪声就更准确了.

找到一篇文章: 

数字信号处理算法(3)-高斯白噪声的产生

http://www.yyearth.com/article/14-05/guass.html

里面讲述了白噪声的产生和C程序的实现,

我直接下载原程序,然后准备用到aar的软件里面, 下面记录一下使用aar编译这个程序的过程:

首先,下载后发现里面包含了四个文件.

blob.png

因为我也是第一次用aar去编译使用C语言,还不熟悉,所以参考最傻瓜式的方法,将三个h文件都放到

E:\aardio\lib\tcc\.res\include 这个目录里面去,这个供你程序里面调用头文件的,

然后把main.c'里面的内容复制到aar编辑器中,准备编译.

注意: 因为用到了printf输出到控制台,所以必须添加

io_reopen();//此句话必须添加才能printf出东西

才能输出东西到控制台.

//aardio调用C语言函数
var code = /****
#include <stdio.h>
#include <stdlib.h> 
//#include <math.h>
#include "sinwn.h"
//C99 __VA_ARGS__
#define dprintf(level, __VA_ARGS__) printf(__VA_ARGS__)


int main(void)
{
int i,m,n;
long seed;
double fs,snr,x[256];
static double a[3]={1,1,1};
static double f[3]={10,17,50};
static double ph[3]={45,10,88};
FILE *fp;
m=3;
n=256;
seed=13332;
fs=150;
snr=5.0;
io_reopen();//此句话必须添加才能printf出东西
sinwn(a,f,ph,m,fs,snr,seed,x,n);
printf("\n Three Sinusoidal Signal plus Gauss White\n");
for(i=0;i<32;i++)
{
printf("    .7lf",x[i]);
if(i%4==3) printf("\n");
}
fp=fopen("sinwn2.dat","w");
for(i=0;i<n;i++)
{
fprintf(fp,".7lf\n",x[i]);
}
fclose(fp);
}

****/

import console;

import tcc;  
console.open()
var vm = tcc( ); //创建TCC编译器 
vm.compile(code); //编译C源码

//调用C函数
//链接并运行C语言main()函数
var ret = vm.run();
 console.log(ret)
vm.close();


console.pause();

上面的C代码完全直接从main.c里面复制的,除了添加了个控制台打开没任何改动!

编译运行.

blob.png

结果完全和博客主说的数据一致,


经实验验证,卡尔曼滤波算法,的确是很好的算法。

要回复文章请先登录注册