求助关于Kalman滤波效果问题【恢复】

LuoYu

目前出现的情况是，水平放置板子原始数据在500(十进制)上下飘动5左右。kalman滤波公式如下：

X(k|k-1)=X(k-1|k-1)     ……… (1)        // 当前预测值=上次最优值 即A=1。控制量U(k)=0

P(k|k-1)=P(k-1|k-1) +Q     ……… (2)

X(k|k)= X(k|k-1)+Kg(k)( Z(k)-X(k|k-1) )   ……… (3)   //就当测量值与实际值对应，H=1，H'=1

Kg(k)= P(k|k-1) / (P(k|k-1) + R)   ……… (4)

P(k|k)=（1-Kg(k)）P(k|k-1)    ……… (5) 

不知道我这样改的想法对么？H A 和I都为1 了。

对应的程序如下： 

float KalmanGain = 0;                    //卡尔曼增益Kg(k)

float EstimateCoveriance = 5;            //估计Coveriance即为P(k|k)

float EstimateValue = 100;               //估计值

float MeasureCovariance = 2;                 //测量Coveriance



float KalmanFilter(float Measure)        //Measure为测量值

{

        //更新估计协方差

        EstimateCoveriance = EstimateCoveriance + (1/10000);

        //计算卡尔曼增益

        KalmanGain = ( EstimateCoveriance/(EstimateCoveriance + MeasureCovariance) );

        //计算本次滤波估计值

        EstimateValue = EstimateValue + KalmanGain*(Measure - EstimateValue);

        //更新估计协方差

        EstimateCoveriance = (1-KalmanGain)*EstimateCoveriance;

        //返回至估计值

        return        EstimateValue;

}

直接利用以上编写kalman函数进行了调用Uart0SendDecimals(KalmanFilter((float)nAdcData));

输出的效果看起来效果好象不错。但是问题来了。当我翻动板子的时候 kalman滤波的数据反应迟钝。跟不过来。觉得KalmanGain没随测量值快速增减。效果图见附件中的[翻动.jpg]。



分别为原始数据、滑动滤波和kalman (原文件名:翻动.jpg) 



我在想是不是参数上出问题了？或者是公式上有问题？其中的MeasureCovariance即测量值误差是不是应该会变动的？各位高手帮忙

LuoYu

重新设定了下参数MeasureCovariance = 1.0f，kalman滤波能跟着走了。但是觉得怎么和滑动滤波差不多- -d 闷到。见下图：





 (原文件名:(E(E@(_QYZF4ZO_@)KHZDZG.jpg) 

jinyudzy

你这样用卡尔曼滤波器相当于一个低通滤波起了。卡尔曼滤波主要是数据融合的一种方法。要测角度就得有陀螺积分的角度和加速度计算的角度进行卡尔曼滤波。

jinyudzy

#include <math.h>



struct Gyro1DKalman

{

        double x_angle;

        double x_bias;



        double P_00;

        double P_01;

        double P_10;

        double P_11;        

        

        double Q_angle; 

        double Q_gyro;

        double R_angle;

};



void init_Gyro1DKalman(struct Gyro1DKalman *filterdata, double Q_angle, double Q_gyro, double R_angle)

{

        filterdata->Q_angle = Q_angle;

        filterdata->Q_gyro  = Q_gyro;

        filterdata->R_angle = R_angle;

}



void ars_predict(struct Gyro1DKalman *filterdata, const double dotAngle, const double dt)

{

        filterdata->x_angle += dt * (dotAngle - filterdata->x_bias);



        filterdata->P_00 +=  - dt * (filterdata->P_10 + filterdata->P_01) + filterdata->Q_angle * dt;

        filterdata->P_01 +=  - dt * filterdata->P_11;

        filterdata->P_10 +=  - dt * filterdata->P_11;

        filterdata->P_11 +=  + filterdata->Q_gyro * dt;

}



double ars_update(struct Gyro1DKalman *filterdata, const double angle_m)

{

        const double y = angle_m - filterdata->x_angle;

        

        const double S = filterdata->P_00 + filterdata->R_angle;

        const double K_0 = filterdata->P_00 / S;

        const double K_1 = filterdata->P_10 / S;

        

        filterdata->x_angle +=  K_0 * y;

        filterdata->x_bias  +=  K_1 * y;

        

        filterdata->P_00 -= K_0 * filterdata->P_00;

        filterdata->P_01 -= K_0 * filterdata->P_01;

        filterdata->P_10 -= K_1 * filterdata->P_00;

        filterdata->P_11 -= K_1 * filterdata->P_01;

        

        return filterdata->x_angle;

}



double PredictAccG_roll(double a_z, double a_y, double a_x)

{

        return -(atan2(-a_z, a_y)-(3.14159/2.0));

}



void main(void)

{

        double tmp, roll_angle;

        double accelero_x,accelero_y,accelero_z;

        double gyro_roll_rad;

        double dt=20;

        struct Gyro1DKalman filter_roll;

        

        init_Gyro1DKalman(&filter_roll, 0.0001, 0.0003, 0.69);



        while(1)

        {

                accelero_x=rand();

                accelero_y=rand();

                accelero_z=rand();

                gyro_roll_rad=rand();



                tmp = PredictAccG_roll(accelero_z, accelero_y, accelero_x);



                ars_predict(&filter_roll, gyro_roll_rad, dt);



                roll_angle = ars_update(&filter_roll, tmp); 

        }

}

zhengrob

楼上,看来你也研究过 TOM Pyckebe的 那个Kalman demo application。请教一个问题，  init_Gyro1DKalman(&filter_roll, 0.0001, 0.0003, 0.69)中的0.0001, 0.0003, 0.69 这几个参数你有没有实际调整过。如果有的话请谈一下如何优化这些参数来达到一个理想的滤波效果。

346675655

 我正在找这个资料呢   太感谢楼主了

 想用于GPS漂点滤除功能...

 楼主给点建议

 非常感谢

jinyudzy

其实我也没怎么研究过，对于这几个参数的调整，我现在也有点糊涂。目前我主要是在滤波器不发散的前提下尽量的减小陀螺误差加大加速度的误差。

feng_matrix

楼主算法仅仅是个积分方式的单入单出滤波而已，单道KALMAN滤波对惯性导航来说意义不大，【3楼】的用法正确

惯导算法需要在不同的运动状态下（轴旋转运动与轴向变速运动），动态的调整加速度与陀螺仪对输出真实角度的贡献权值

KAMMAN滤波正好符合这种要求，所以成为了惯性导航经典算法

sevenlion

请教个问题，这样的滤波是用陀螺积分的角度和加速度计测的倾角进行数据融合。

如果要检测的角度是水平面内的转动，那用两个加速度计也只能测出角加速度，怎样才能测出转动的角度呢？



难道只能是加速度计测出的角加速度积分得到的角速度和陀螺测得的角速度做融合，然后再积分出角度？

litao8421

经典惯导，需要你建立动态误差方程，而且通常情况下，加表是提供速度信息，陀螺提供姿态信息。 你目前的kf和一般的加权平均没有差别，因为你没有用到动态模型，这个时候的kalman filter会退化成least squre，再进一步简化，其实就是加权平均了。这个就是你图二的原因，所以这个时候你是没有必要用kalman filter的。



用Kalman Filter一定要用上他的系统模型，建立误差方程，

v-aren

3楼的算法有没详细的说明啊，看不懂

baiya

mark

baiya

回复【2楼】jinyudzy 高月山
-----------------------------------------------------------------------

有没有更详细的资料，对我很有帮助，但是没有完全看懂，谢谢

fanwt

mark~~

yangyh75

楼主能发个图形监控软件使使吗？谢了

fengleihu

回复【3楼】jinyudzy 高月山
-----------------------------------------------------------------------

马克

asha

需要有状态量和观测量，一般这两个量不是同一个传感器的输出。而且，这种用法增益K好像成了个固定值了。虽然有把滤波之后的数字作为观测量的，但是用的模型是HMM模型。关于那几个参数，是观测量和状态量的误差阵。

kevinfzh

mark

wers_l

好

jmp2002911

MARK

cqlutao

Mark

jacknupt

mark~看code~

jade0606

你使用
X(k|k-1)=X(k-1|k-1)
就是X(k)=Fk X(k-1)+Bk Uk+ wk, 中的Fk=1,
这种使用方法最后卡尔曼滤波就是权值不等的算数平均数,所以你会有那个结果

jiajiajia

mark！

gongyuan1

mark~

suxkd1

MeasureCovariance（测量协方差） 代表的是什么意思呢？