智能车制作

标题: 卡尔曼滤波用的是非矩阵还是矩阵的? [打印本页]

作者: subingkiun 时间: 2013-1-24 13:52
标题: 卡尔曼滤波用的是非矩阵还是矩阵的?
本帖最后由 subingkiun 于 2013-1-24 13:53 编辑

在网上找到的非矩阵的卡尔曼滤波的程序,非矩阵的,有人用他直立过么?

volatile float QingJiao = 0;
volatile float Gyro_Data = 0;
void Kalman(void)
{
float Q =1,R = 300;
static float RealData = 0,RealData_P = 0;
float NowData = 0,NowData_P = 0;
float Kg = 0,gyroscope_rate = 0,accelerometer_angle=0;
float Acc_x = 0,Acc_z = 0, Gyro = 0;
static float his_acc = 0.0,his_accx = 0.0,his_accz = 0.0;

while(!ATD0STAT0_SCF); //等待转换完成

Acc_x = (float)ATD0DR1;
Acc_z = (float)ATD0DR2;
Gyro  = (float)ATD0DR0;

if(Gyro > 4090) Gyro += 1000;
else
if(Gyro > 4084) Gyro += 500;

if(Gyro < 55) Gyro -= 1000;
else
if(Gyro < 60) Gyro -= 500;

Acc_x = Acc_x - 2342.0;
Acc_z = Acc_z - 2076.0;
Gyro  = Gyro  - 2048.0;
Gyro_Data = Gyro;

OutData[0] = Gyro_Data;
accelerometer_angle = atan2f(-Acc_x,Acc_z);
OutData[1] = accelerometer_angle*1000;

gyroscope_rate = Gyro*0.0023;       //参考电压3.3v 12位ADC 放大9.1倍 enc-03 0.67mv/deg./sec.

                                       //(3300/4096)/(0.67*9.1)*(3.14/180) =  0.0023

NowData = RealData + gyroscope_rate*0.01;
//1.预估计 X(k|k-1) = A(k,k-1)*X(k-1|k-1) + B(k)*u(k)
NowData_P = sqrt(Q*Q+RealData_P*RealData_P);
//2.计算预估计协方差矩阵 P(k|k-1) = A(k,k-1)*P(k-1|k-1)*A(k,k-1)'+Q(k)
Kg = sqrt(NowData_P*NowData_P/(NowData_P*NowData_P+R*R));
//3.计算卡尔曼增益矩阵 K(k) = P(k|k-1)*H(k)' / (H(k)*P(k|k-1)*H(k)' + R(k))
RealData = NowData + Kg*(accelerometer_angle - NowData);
//4.更新估计 X(k|k) = X(k|k-1)+K(k)*(Z(k)-H(k)*X(k|k-1))
RealData_P = sqrt((1-Kg)*NowData_P*NowData_P);
//5.计算更新后估计协防差矩阵 P(k|k) =（I-K(k)*H(k)）*P(k|k-1)

QingJiao =  RealData;

OutData[2] = QingJiao*1000;
OutPut_Data();
}
他用的AD转换是12位精度的。但是输出的波形是这样的
那我要调什么参数？
是不是陀螺仪的比例大了？
真心求指教！！！
红色的是陀螺仪
黄色是加速度的角度
蓝色的是融合后的。

作者: subingkiun 时间: 2013-1-24 13:54
国际惯例，沙发

求指教

作者: yukunlinykl 时间: 2013-1-24 14:20
呀！还有矩阵版本。
学到了，学到了。

作者: 为了明天_For_To 时间: 2013-1-24 14:31
两个都试过不行要么过冲非常大要么没过冲就是太滞后了，完全不能接受，最后还是换成互补滤波了

作者: hmit 时间: 2013-1-24 14:41
本帖最后由 hmit 于 2013-1-25 19:25 编辑

这卡尔曼被简化的有点厉害。A，B，H，I都是1了。
RealData = NowData + Kg*(accelerometer_angle - NowData);
^                            ^                   ^                ^
|                            |                      |                   \
最优估计值             Q/（Q+R）    观测值（加速度计）    估计值（陀螺仪）

你的程序 Q =1,R = 300;这样Kg很小几乎不起作用。懂了吗？

作者: subingkiun 时间: 2013-1-24 21:05

hmit 发表于 2013-1-24 14:41
这卡尔曼被简化的有点厉害。A，B，H，I都是1了。
RealData = NowData + Kg*(accelerometer_angle - NowDat ...

是要调Q跟R的是么？
dt要调么？
今天下午看了一下这个资料。
大概知道怎么个流程。
现在调出了这样的波形，不知道行不行。。
蓝色是滤波后的；
黄色是加速度的输出；
红色的是陀螺仪的输出。
希望指出不足，

作者: 、煈の°絮だ 时间: 2013-1-24 23:18
滞后有点严重啊

作者: 、煈の°絮だ 时间: 2013-1-24 23:21

subingkiun 发表于 2013-1-24 21:05
是要调Q跟R的是么？
dt要调么？
今天下午看了一下这个资料。

这个真不错啊

作者: hmit 时间: 2013-1-25 18:53
本帖最后由 hmit 于 2013-1-25 19:19 编辑

subingkiun 发表于 2013-1-24 21:05
是要调Q跟R的是么？
dt要调么？
今天下午看了一下这个资料。

差不多了。只不过陀螺仪最后的一段波形怎么回事？
dt是你调用卡尔曼滤波的周期。0.005就是5ms调用一次卡尔曼滤波。你10ms一次有点慢。

作者: hmit 时间: 2013-1-25 19:02
还有你的陀螺仪信号与角度信号极性反了。在调PD要加个负号。还是推荐用矩阵的那个。他可以把角速度也很好的滤一下。

作者: hmit 时间: 2013-1-25 19:24
还有一个问题，你波特率多少。9600的话就不要把 “OutPut_Data();”放到中断里，太慢了。

作者: №、恋い猪 时间: 2013-3-13 14:03
请问LZ，用非矩阵的卡尔曼滤波，你们的小车站起来了吗？

作者: subingkiun 时间: 2013-3-13 15:59

hmit 发表于 2013-1-25 19:24
还有一个问题，你波特率多少。9600的话就不要把 “OutPut_Data();”放到中断里，太慢了。

是的,我们也发现了.
放里面运行的好慢.

作者: subingkiun 时间: 2013-3-13 16:00

№、恋い猪发表于 2013-3-13 14:03
请问LZ，用非矩阵的卡尔曼滤波，你们的小车站起来了吗？

可以的.
但是Q跟R的值要改改。

作者: jeyran 时间: 2013-3-16 17:55
楼主用哪一款的陀螺仪?

作者: subingkiun 时间: 2013-3-16 17:58

jeyran 发表于 2013-3-16 17:55
楼主用哪一款的陀螺仪?

官方不是规定了用村田公司的么?

作者: jeyran 时间: 2013-3-16 18:01
村田的哪一款

作者: subingkiun 时间: 2013-3-16 18:12

jeyran 发表于 2013-3-16 18:01
村田的哪一款

是哪家公司做的模块吧?
我们用的是自己做的

作者: huayuc19 时间: 2013-3-19 20:20
想请教下楼主卡尔曼里面的dt怎么改？

作者: huayuc19 时间: 2013-3-19 20:21
还有看LZ 贴的程序用到了X轴。想问下呢个求atan2f求的角度有什么用？

作者: Idenktsie 时间: 2013-4-9 10:33
Acc_x = Acc_x - 2342.0;
   Acc_z = Acc_z - 2076.0;
   Gyro  = Gyro  - 2048.0;
   Gyro_Data = Gyro;
这些数据怎么来的啊？？

作者: Idenktsie 时间: 2013-4-9 20:13
这个float Acc_x = 0,Acc_z = 0, Gyro = 0;是定义什么？？

作者: huangjianwu 时间: 2013-4-17 19:45

作者: 帅气、灰太郎 时间: 2015-1-17 20:20
Acc_x = (float)ATD0DR1;
Acc_z = (float)ATD0DR2;
Gyro = (float)ATD0DR0;

请问下 Acc_x,Acc_z是x轴和z轴加速度吗？
还有平衡车只要一个加速度计就行了。。。。。
不理解。。。。。

欢迎光临智能车制作 (http://111.231.132.190/)