关于卡尔曼算法
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 = {{ 1, 0 },{ 0, 1 }};
static float Pdot ={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=Q_angle - P - P;
Pdot=- P;
Pdot=- P;
Pdot=Q_gyro;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
angle_err = angle_m - angle;
PCt_0 = C_0 * P;
PCt_1 = C_0 * P;
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;
P -= K_0 * t_0;
P -= K_0 * t_1;
P -= K_1 * t_0;
P -= 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 于 2012-8-24 13:12 编辑
const修饰表示常量
static表示静态,静态局部变量只在它的语句块中可以使用。
静态全局变量只能在本文件使用,其作用域从声明位置到文件结束。
在这个例子中,static const是全局静态常量。
另外,对于全局变量,是否赋初值,对最终的可执行文件的大小是有影响的。
例如:
static int g_nArray; /* 声明变量,生成可执行文件时不会预留1000*sizeof(int)的大小
只有当进程运行时才会在内存中为该变量分配 1000*sizeof(int)的大小空间 */
static int g_nArray = {1};/* 声明并定义,进行了初始化,生成可执行文件时会分配1000*sizeof(int)的大
小给该变量。*/
是否使用static只是限制变量的使用范围。 xfgasa 发表于 2012-8-24 12:58 static/image/common/back.gif
const修饰表示常量
static表示静态,静态局部变量只在它的语句块中可以使用。
静态全局变量 ...
所以,不要进行没有必要的初始化。 xfgasa 发表于 2012-8-24 13:13 static/image/common/back.gif
所以,不要进行没有必要的初始化。
大哥。。能不能说说那几个数组的作用。
页:
[1]