讨论一个卡尔曼滤波入门问题

玉面飞龙

前几天在网上看到了一个卡尔曼的介绍，自己虽然有数字信号处理的基础，但是毕竟底子比较薄弱，还没有完全弄明白详细的机理，但是大概的过程是明白了。
基本就是用上一次的最优估计值作为真实值进行数据迭代，然后不断地把数据误差缩小。
这里贴上代码，和百度文库的很像，我只是修改了迭代次数。
#include "stdio.h"
#include "stdlib.h"
#include "math.h"
double frand() 
{      return 2*((rand()/(double)RAND_MAX) - 0.5);  //随机噪声
}  
void main()
{ float x_last=0;   
float p_last=0.02;
float Q=0.018;
float R=0.542;
float kg;
float x_mid;
float x_now;
float p_mid;
float p_now;
float z_real=0.56;//0.56
float z_measure;
float sumerror_kalman=0;
float sumerror_measure=0;
int i;
x_last=z_real+frand()*0.03;
x_mid=x_last;
for(i=0;i<400;i++)
{ x_mid=x_last;    //x_last=x(k-1|k-1),x_mid=x(k|k-1)
p_mid=p_last+Q;  //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
kg=p_mid/(p_mid+R); //kg为kalman filter，R为噪声
z_measure=z_real+frand()*0.03;//测量值
x_now=x_mid+kg*(z_measure-x_mid);//估计出的最优值
p_now=(1-kg)*p_mid;//最优值对应的covariance
printf("Real     position: %6.3f \n",z_real);  //显示真值
        printf("Mesaured position: %6.3f [diff:%.3f]\n",z_measure,fabs(z_real-z_measure));  //显示测量值以及真值与测量值之间的误差
        printf("Kalman   position: %6.3f [diff:%.3f]\n",x_now,fabs(z_real - x_now));  //显示kalman估计值以及真值和卡尔曼估计值的误差
        sumerror_kalman += fabs(z_real - x_now);  //kalman估计值的累积误差
        sumerror_measure += fabs(z_real-z_measure);  //真值与测量值的累积误差
        p_last = p_now;  //更新covariance值
        x_last = x_now;  //更新系统状态值
}
printf("总体测量误差     : %f\n",sumerror_measure);  //输出测量累积误差
    printf("总体卡尔曼滤波误差: %f\n",sumerror_kalman);   //输出kalman累积误差
    printf("卡尔曼误差所占比例: %d%% \n",100-(int)((sumerror_kalman/sumerror_measure)*100)); 
}
运行出来的结果是，比20次的迭代数据误差波动要小一些，但并不是小很多。这是400次的迭代结果。rand是伪随机函数，过两天我把用STM32随机数发生器做的测试代码发上来，顺便测试一下效率。不过PC机跑了一下，感觉不怎么好。
现在一个问题是，真实值我是不知道的，那么刚开始的输入我应该怎么写呢？是0还是随便的一个数据，希望能和大家一起讨论一下！谢谢！

正点原子

帮顶。。。。

玉面飞龙

回复【2楼】正点原子:
---------------------------------
原子哥，问题已经找到。
卡尔曼滤波是一种最优滤波方案，只是在均方误差最小的准则下，加入了一个方差最小的准则。
实测的波动还是有的不过在±1度左右，做飞行器的话问题应该不是很大。
估计还是估计，毕竟永远无法测得真实值。这种随机迭代，基本上要达到3000多次以上好像才会收敛。贴上代码：

void Kalman_yaw()
{
static float x_last=0;//????????????????????
static float p_last=0.02;
static float Q=0.018;
static float R=0.542;
static float Kg;//Kalman Gain;
static float x_mid;
static float x_now;
static float p_mid;
static float p_now;
 //float z_real=Copter.roll;
static float z_measure;
// float sumerror_kalman=0;
//float sumerror_measure=0;
//int i=0;
// x_last=z_real;
//x_mid=x_last;

x_mid=x_last;
p_mid=p_last+Q;
Kg=p_mid/(p_mid+R);
z_measure=Copter.yaw;
x_now=x_mid+Kg*(z_measure-x_mid);
p_now=(1-Kg)*p_mid;
p_last=p_now;
x_last=x_now;//??????????????×÷??×????
Copter.yaw=x_now;//?ü?? ·????÷?ó??????????×÷?????????ù??
}
这里的Copter.yaw是利用6050读出来的yaw.数据波动还行，在0.01度这样

玉面飞龙

希望能做一个自适应的东西，不知道论坛里的人有没有新的想法

惯性坐标系

我们可以深入交流一下卡尔曼滤波的含义及其实现，一般的处理在要求比较高的飞行器上还欠缺。

玉面飞龙

回复【5楼】惯性坐标系:
---------------------------------
实验我已经做完了，感觉加入的效果并没有6050本身的DMP好。
因为是一个估计，卡尔曼就是用上次的估计值作为准确值反复迭代最后毕竟真实值的
这个好处是在毕竟的同时相比较于维纳滤波器多一个方差最小的有点，不过还是有波动的。波动比较小，最后在一个收敛区间里面反复波动了。
目前了解的就是这些。在自动控制这块倒是着实很头疼。不知道该用什么方法了。

XINSI

回复【6楼】玉面飞龙:
---------------------------------
以前想搞卡尔曼，发现完全搞不懂。。。。。。。

玉面飞龙

回复【7楼】XINSI:
---------------------------------
原理仔细想想还是能够明白的，
就是把上一次的估计值当做真实的值反复迭代，最终能够逼近。
但是传感器自己出了有噪声，还有一些量化误差的东西，这些会导致最后的数据是在一个区间里面波动。
至于为什么能够这样做，那就有相应的理论推导，证明我们这样做是可以的。
优点是不仅能达到和维纳滤波器一样的效果，同时数据波动的方差还是最小的。
缺点是需要迭代一定的周期，这个对于快速系统和慢的传感器是不合适的。

今天天气不好

你这不是卡尔曼吧，你这是互补滤波吧

979653421@qq.co

楼主有没有开动过电机测试dmp抗干扰效果怎么样，我移植的，1/2油门就开始大幅度波动了有3. 4度，不知楼主是否遇到过类似问题

玉面飞龙

回复【9楼】今天天气不好:
---------------------------------
和互补滤波不一样的，上面有更新的。

玉面飞龙

回复【10楼】979653421@qq.com:
---------------------------------
 没，电机还没测试，不过如果是抗干扰效果不好，是不是EMI问题？硬件设计，可能要多注意下把。

979653421@qq.co

回复【12楼】玉面飞龙:
---------------------------------
你说的emi是指滤波吗

2013的弹子球

回复【9楼】今天天气不好:
---------------------------------
他这个最后效果跟互补没有区别呢~~~~~~~最后叠来叠去参数就固定了。。。。。跟互补就一样了。。。。

玉面飞龙

回复【14楼】2013的弹子球:
---------------------------------
可能和阶数有关吧，我感觉这个东西还是在处理随机问题上，效果要好些。
6050那个传感器也不怎么样，做怎么样效果也不会好到哪里去。

玉面飞龙

回复【13楼】979653421@qq.com:
---------------------------------
也有可能是震动问题了。

小白110

回复【3楼】玉面飞龙:
---------------------------------
// /*卡尔曼滤波参数*/
 static float angle, angle_dot;  //外部需要引用的变量
static float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;   //注意：dt的取值为kalman滤波器采样时间;, dt=0.01
static float [2][2] = {{ 1, 0 },{ 0, 1 }};
static float dot[4] ={0,0,0,0};
static char C_0 = 1;
static float q_bias, angle_err, Ct_0, Ct_1, E, K_0, K_1, t_0, t_1;

void Kalman_Filter(float angle_m,float gyro_m)
{
angle+=(gyro_m-q_bias) * dt;//先验估计

Pdot[0]=Q_angle - [0][1] - [1][0];// k-' 先验估计误差协方差的微分
Pdot[1]=-P[1][1];
Pdot[2]=-P[1][1];
Pdot[3]=Q_gyro;

P[0][0] += dot[0] * dt;// k- 先验估计误差协方差微分的积分 = 先验估计误差协方差
P[0][1] += dot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;

angle_err = angle_m - angle;//zk-先验估计

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;//Kk
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 angle;
//return angle_dot;
}

师父，我的卡尔曼滤波的波形过冲，怎么处理啊？

小白110

// /*卡尔曼滤波参数*/
 static float angle, angle_dot; //外部需要引用的变量
static float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;   //注意：dt的取值为kalman滤波器采样时间;, dt=0.01
static float P[2][2] = {{ 1, 0 },{ 0, 1 }};
static float Pdot[4] ={0,0,0,0};
static char C_0 = 1;
static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

void Kalman_Filter(float angle_m,float gyro_m)
{
angle+=(gyro_m-q_bias) * dt;//先验估计

Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分
Pdot[1]=-P[1][1];
Pdot[2]=-P[1][1];
Pdot[3]=Q_gyro;

P[0][0] += Pdot[0] * dt;// Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;

angle_err = angle_m - angle;//zk-先验估计

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;//Kk
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 angle;
//return angle_dot;
}

海文

各位大神，要怎么验证精度啊？

leasun

卡尔曼滤波跟迭代没什么关系，他是通过观测误差最优化估计线性系统误差的方法。你这个如果是卡尔曼滤波的话，我想问问你的状态方程是什么？量测方程呢？

tao475824827

小白110 发表于 2015-8-10 11:44
// /*卡尔曼滤波参数*/
&nbsp;static float angle, angle_dot;                 //外部需要引用的变量
static float Q_a ...

请教一下这段程序中间，Pdot矩阵和P矩阵那部分运算是什么意思啊

fang百川

帮顶

13969873363

小白110 发表于 2015-8-10 11:44
// /*卡尔曼滤波参数*/
&nbsp;static float angle, angle_dot;                 //外部需要引用的变量
static float Q_a ...

请问这是什么软件，是不是也能观察PID的优化程度

薛定谔的猫

惯性坐标系 发表于 2015-5-13 11:26
我们可以深入交流一下卡尔曼滤波的含义及其实现，一般的处理在要求比较高的飞行器上还欠缺。

你好，能交流一下使用卡尔曼滤波的一些心得吗，就是在四轴上用的
企鹅1375127228

Wanhh

小白110 发表于 2015-8-10 11:43
回复【3楼】玉面飞龙:
---------------------------------
//&nbsp;/*卡尔曼滤波参数*/
&nbsp;static&nbsp; ...

滤波时间没调好

Wanhh

小白110 发表于 2015-8-10 11:44
// /*卡尔曼滤波参数*/
&nbsp;static float angle, angle_dot;                 //外部需要引用的变量
static float Q_a ...

这个能将角速度滤到什么程度？能达到0.01精度吗？

1723068930wm

Wanhh 发表于 2020-5-25 11:17
滤波时间没调好

滤波时间是常数，和这段代码运行周期对应，不需要调