|
发表于 2012-12-12 11:09:44
|
显示全部楼层
本帖最后由 johnsonzzd 于 2012-12-12 11:17 编辑
你的数据,9列各是什么东西?
数学模型:角速度积分得出角度。角速度传感器的零漂等对应模型噪声Q,这个值要小。模型不准,啥都白搭。
测量:加速度值测出来的角度。测量值的方差就是测量方差R。这个值大小不重要,估计一下就行了。
你的温度例子有点复杂,给你写一个简单的便于理解 - clc; clear
- N=200;
- for t=1:N
- z(t)=25+3*randn; %测量值
- end
- R=std(z)^2 %测量噪声
- V=0.1*randn(1,N);
- Q=std(V)^2 %模型噪声
- p(1)=10;
- x(1)=0;
- for t=2:N;
- x1(t)=x(t-1); %预测状态
- p1(t)=p(t-1)+Q; %预测估计协方差
-
- k(t)=p1(t)/(p1(t)+R); %最优卡尔曼增益
- x(t)=x1(t)+k(t)*(z(t)-x1(t)); %更新状态估计
- p(t)=(1-k(t))*p1(t); %更新协方差估计
- end
- t=1:N;
- plot([0,N],[25,25],'g',t,z,'b',t,x,'r');
- legend('真实值','测量值','估计值');
复制代码
最后,MPU6050内部自己有算法,能够直接取出角度,不需要自己再写卡尔曼之类的程序了。 |
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
|