二阶线性微分器的实现
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. 结果
中间红色为二阶微分器输出,下面为一阶微分器输出,干扰噪声得到明显抑制。