#include <stdio.h>


float Q_angle=0.001;//过程噪声的协方差
float Q_gyro=0.003;//0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
float R_angle=0.5;// 测量噪声的协方差 既测量偏差
float dt=0.005;//注意：dt的取值为kalman滤波器采样时间;
float P[2][2] = {{ 1, 0 },{ 0, 1 }};
float Angle,Angle_dot;
float Pdot[4] ={0,0,0,0};
const char C_0 = 1;
float Q_bias, Angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
......................

#include <stdio.h>
/************************************/
//增量式PID
/*************************************/

//初始化变量
struct _PID{
    float SetSpeed;       //设定值
    float ActualSpeed;    //检测值
    float Err;            //当前误差
    float Err_last;       //上一次误差
    float Err_llast;      //上上次的误差
......................

#include <stdio.h>
/************************************/
//位置式PID
/*************************************/

//初始化变量
struct _PID{
    float SetSpeed;       //设定值
    float ActualSpeed;    //检测值
    float Err;            //当前误差
    float Err_last;       //上一次误差
    float KP,KI,KD;       //PID参数
......................