关于卡尔曼算法

觋-拂晓

float angle, angle_dot ;         //外部需要引用的变量
//-------------------------------------------------------
static const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.1, dt=0.01;
                        //注意：dt的取值为kalman滤波器采样时间;
static float P[2][2] = {{ 1, 0 },{ 0, 1 }};

static float Pdot[4] ={0,0,0,0};

static const char C_0 = 1;

static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//-------------------------------------------------------
float Kalman_Filter(float angle_m,float gyro_m)                        //gyro_m:gyro_measure
{
        angle+=(gyro_m-q_bias) * dt;
        Pdot[0]=Q_angle - P[0][1] - P[1][0];
        Pdot[1]=- P[1][1];
        Pdot[2]=- P[1][1];
        Pdot[3]=Q_gyro;

        P[0][0] += Pdot[0] * dt;
        P[0][1] += Pdot[1] * dt;
        P[1][0] += Pdot[2] * dt;
        P[1][1] += Pdot[3] * dt;

        angle_err = angle_m - angle;

        PCt_0 = C_0 * P[0][0];
        PCt_1 = C_0 * P[1][0];

        E = R_angle + C_0 * PCt_0;

        K_0 = PCt_0 / E;
        K_1 = PCt_1 / E;

        t_0 = PCt_0;
        t_1 = C_0 * P[0][1];

        P[0][0] -= K_0 * t_0;
        P[0][1] -= K_0 * t_1;
        P[1][0] -= K_1 * t_0;
        P[1][1] -= K_1 * t_1;

        angle        += K_0 * angle_err;
        q_bias        += K_1 * angle_err;
        angle_dot = gyro_m-q_bias;
    return 1;
}
谁能跟我说说各个static变量的含义，有什么作用。求指教。

夏日么么茶

static是静态全局变量的意思，静态全局变量在声明它的整个文件都是可见的，而在文件之外是不可见的

xfgasa

const修饰表示常量
static表示静态，静态局部变量只在它的语句块中可以使用。
               静态全局变量只能在本文件使用，其作用域从声明位置到文件结束。
在这个例子中，static const是全局静态常量。

另外，对于全局变量，是否赋初值，对最终的可执行文件的大小是有影响的。

例如：

1. 
   
2. static int g_nArray[1000];           /* 声明变量，生成可执行文件时不会预留1000*sizeof(int)的大小
   
3.                                   只有当进程运行时才会在内存中为该变量分配 1000*sizeof(int)的大小空间 */
   
4. static int g_nArray[1000] = {1};  /* 声明并定义，进行了初始化，生成可执行文件时会分配1000*sizeof(int)的大
   
5.                                   小给该变量。*/
   

复制代码

是否使用static只是限制变量的使用范围。

xfgasa

xfgasa 发表于 2012-8-24 12:58
const修饰表示常量
static表示静态，静态局部变量只在它的语句块中可以使用。
               静态全局变量 ...

所以，不要进行没有必要的初始化。

觋-拂晓

xfgasa 发表于 2012-8-24 13:13
所以，不要进行没有必要的初始化。

大哥。。能不能说说那几个数组的作用。