自己写的卡尔曼数据融合,附波形图和代码
先上图吧红色是陀螺仪数据,蓝色是加速度计数据,绿色是融合后的结果。大家看看这个效果能行么? 然后是代码。 public class Kalman{
static double trueValue = 0;
static double offset = 1; //最优值偏差(最好不为零,否则可能不收敛)
static double Kg1 = 0.8; //卡尔曼增益
static double X10 = 0, P10 = 0.1; // X(k|k-1),P(k|k-1)
static double X11 = 0, P11 = 0.1; // X(k|k),P(k|k)
static double R = 0.1; //不确定度
static double Q = 0.01; //
static public double Filter(double gyo, double acc)
{
X10 = gyo;
P10 = P11 + Q;
X11 = X10 + Kg1 * (acc - X10);
Kg1 = P10 / (P10 + R);
P11 = (1 - Kg1) * P10;
return X11;
}
} 这个是单轴的吧,三轴你要怎么做 my_ve 发表于 2013-11-5 22:10 static/image/common/back.gif
这个是单轴的吧,三轴你要怎么做
在数学上是把那些值放在一个3*3的矩阵里去算,不过用C写算法的话重复调用一下Filter方法就行了~感觉这样还方便一些,毕竟Filter方法里的两个参数就是acc和gyo的数据 这是acc角度和gyo角度的融合吗?看不懂{:dizzy:}
三轴是三个分开算?那三轴的耦合怎么办呢 mark mute 发表于 2013-11-6 14:10 static/image/common/back.gif
这是acc角度和gyo角度的融合吗?看不懂
三轴是三个分开算?那三轴的耦合怎么办呢 ...
三轴的耦合是什么意思啊?{:3_48:} maochonghao 发表于 2013-11-5 23:13 static/image/common/back.gif
在数学上是把那些值放在一个3*3的矩阵里去算,不过用C写算法的话重复调用一下Filter方法就行了~感觉这样 ...
有点不对,如果你测量的是pitch,那你从水平开始,做roll 80度左右,yaw 80度左右,注意pitch尽量不要转动,你再看下曲线的跟随情况~ 学习了C#? my_ve 发表于 2013-11-6 21:53 static/image/common/back.gif
有点不对,如果你测量的是pitch,那你从水平开始,做roll 80度左右,yaw 80度左右,注意pitch尽量不要转 ...
那就是四元数要讨论的问题了吧~现在只在数据融合的处理,所以我是用的C#里的random类模拟出来的传感器数据。 真正实测的波形我会在后面再贴出来分享。 william_rain 发表于 2013-11-6 21:55 static/image/common/back.gif
学习了C#?
对呀自己做上位机,感觉这样开发更上手 maochonghao 发表于 2013-11-7 15:48 static/image/common/back.gif
那就是四元数要讨论的问题了吧~现在只在数据融合的处理,所以我是用的C#里的random类模拟出来的传感器数 ...
就是姿态结算,用不用四元数都无所谓~ maochonghao 发表于 2013-11-7 15:48 static/image/common/back.gif
那就是四元数要讨论的问题了吧~现在只在数据融合的处理,所以我是用的C#里的random类模拟出来的传感器数 ...
double gyo, double acc 这两个是什么单位呢?我理解融合是有关系之间的量纲 mute 发表于 2013-11-11 15:16 static/image/common/back.gif
double gyo, double acc 这两个是什么单位呢?我理解融合是有关系之间的量纲
gyo 和 acc 是分别最终由姿态解算得出的陀螺仪和加计算出的pitch和roll最后用kalman滤波得到最优估计咯 这样可以用的 学习一下· 学习一下~明天写到单片机里面试验下 mark
页:
[1]