二阶线性微分器的实现

二阶线性微分器的实现

1. 经典微分器 y = (v(t）- v(t - T))/T在间隔时间 T很小时，对噪声有放大作用，实际无法应用到任务中。

2. 二阶线性微分器

 

 

3. C程序验证

#include <stdio.h>
#include <stdlib.h>
#include "math.h"
#include "stdlib.h"

#define TS (double)(0.01)   /*采样步长*/
#define TN (double)(0.05)   /*微分步长*/

double Erjie(double x1,double x2,double u)
{
    double ret;
    double deta;

    deta = 1.0 / (TN * TN);
    ret = deta * (x1 - u) + x2 * 2 / TN;

    return -ret;
}

int main()
{
    int i = 0;
    double tim,dv,ds;
    double u_1;
    double x1,x1_1,x2,x2_1;
    FILE *fp,*fnp,*fdp;

    x1 = 0;
    x1_1 = 0;
    x2 = 0;
    x2_1 = 0;
    srand(2);
    if((fp = fopen("fst_yuan.txt","w+")) == NULL)
    {
        printf("fail to open fst_yuan.txt\n");
        exit(0);
    }

    if((fnp = fopen("fst_now.txt","w+")) == NULL)
    {
        printf("fail to open fst_now.txt\n");
        exit(0);
    }
    if((fdp = fopen("fst_dev.txt","w+")) == NULL)
    {
        printf("fail to open fst_dev.txt\n");
        exit(0);
    }

    for(i = 0; i < 1000;i++)
    {
        tim = TS * (i+1);
        
        ds = sin(tim) + 0.05 * (rand()/32768.0);  /*模拟的噪声*/
        dv = cos(tim);                            /*理想的微分曲线*/

        x1_1 = x1 + TS * x2;                      /*微分跟踪*/
        x2_1 = x2 + TS * Erjie(x1,x2,ds);

        fprintf(fp,"%.10lf    %.10lf\n",ds,dv);   /*保存到文件。通过Matlab 绘制曲线*/
        fprintf(fnp,"%.10lf    %.10lf\n",x1_1,x2_1);
        fprintf(fdp,"%.10lf    %.10lf\n",x1_1,(ds - u_1)/TS);
        x1 = x1_1;
        x2 = x2_1;
        u_1 = ds;
    }
    fclose(fp);
    fclose(fnp);
    fclose(fdp);
    printf("Hello world!\n");
    return 0;
}

4. Matlab 绘图 

load('C:\\Users\TerryC\Documents\codeblocks\微分器\fst_yuan.txt');
load('C:\\Users\TerryC\Documents\codeblocks\微分器\fst_now.txt');
load('C:\\Users\TerryC\Documents\codeblocks\微分器\fst_dev.txt');
subplot(3,1,1);
plot(fst_yuan);
grid on;
subplot(3,1,2)
plot(fst_now);
grid on;
subplot(3,1,3)
plot(fst_dev);
grid on;

5. 结果 

 中间红色为二阶微分器输出，下面为一阶微分器输出，干扰噪声得到明显抑制。