用重力标定加速度计
本帖最后由 duan123 于 2013-11-2 11:43 编辑原文：blog.sina.com.cn/s/blog_81f1e26801017aji.html
这几天都在标定加速度计，终于完成了。
[*]方案
传感器总会有缺陷，测量的值有误差，所以要校正。要校正加速度计，最简单有效的办法就是利用重力了。以不同方向在静止状态测量加速度，然后把数据拟合到当地重力加速度，就完成对加速度计的标定了，直接无视Datasheet上的精度参数。
[*]采集数据
采集数据就要用到上次做的“黑匣”了，测量到的部分数据如下。
http://cache.amobbs.com/new2012/forum/201206/15/194204zioxxo0he49xz4xh.png.thumb.jpg 2012-6-15 19:42 上传
下载附件 (161.48 KB)
里面的0x55AA是帧开始符，表示一组测量数据的开始，紧接着就是3个double类型的值，小端格式，表示x,y,z方向的原始测量值。
[*]理论依据
这次标定加速度计，最大的收获是——把困扰多年的“最小二乘法”弄懂了。
设x‘,y’,z‘为实际加速度分量，G0为当地重力加速度值。理想情况下有下面的等式：
http://cache.amobbs.com/new2012/forum/201206/15/194204ax5d1a44xrr1x5r5.png 2012-6-15 19:42 上传
下载附件 (3.96 KB)
但测量有误差，设测量值为，则测量值和实际值可以用下面的公式拟合。拟合方式为线性拟合，其中a',b',c'是比例因子，d',e',f'为偏移因子。通过这条公式，可以把测量值“变成”实际值（下面称作“估计值”），标定加速度计的目标，就是确定这六个参数（下面称作“参数”），使估计值更接近实际值。
http://cache.amobbs.com/new2012/forum/201206/15/1942054yfgc9f6gcg46qyg.png 2012-6-15 19:42 上传
下载附件 (7.42 KB)
代入理想情况的等式，有：
http://cache.amobbs.com/new2012/forum/201206/15/194205dbbnjsjij8nnno8p.png.thumb.jpg 2012-6-15 19:42 上传
下载附件 (8.71 KB)
这分明是椭球嘛，展开得：
http://cache.amobbs.com/new2012/forum/201206/15/194206t6l1newuye1t4nbn.png.thumb.jpg 2012-6-15 19:42 上传
下载附件 (11.32 KB)
为方便推导和运算，写成如下形式：
http://cache.amobbs.com/new2012/forum/201206/15/194206ltfv8pljy01v1lxv.png.thumb.jpg 2012-6-15 19:42 上传
下载附件 (7.75 KB)
比较以上两式，有：（注意要加上系数k）
http://cache.amobbs.com/new2012/forum/201206/15/194207nfcfn9493m96eh69.png 2012-6-15 19:42 上传
下载附件 (31.65 KB)
反过来有：
http://cache.amobbs.com/new2012/forum/201206/15/194209n8zvcb011nz9qm02.png.thumb.jpg 2012-6-15 19:42 上传
下载附件 (42.1 KB)
现在已经在静止状态测量出一些数据，记作，表示第n组测量值，对应的实际值都是重力G0。为了衡量参数的合适程度，引入误差函数Δn，表示第n个估计值与实际值的偏差，如果硬要说出具体意义，就是：估计值和实际值的平方差的1/k倍。 http://cache.amobbs.com/new2012/forum/201206/15/194209pqvtp0o1dwzgdpd9.png.thumb.jpg 2012-6-15 19:42 上传
下载附件 (8.26 KB)
Δn是第n组数据的偏差，我们的目标是整体偏差最小，所以引入整体偏差指数I，等于全部数据的偏差的平方和（“最小二乘法”的“二乘”就是指平方）。 http://cache.amobbs.com/new2012/forum/201206/15/1942095rr5rrfbff4ruvdr.png 2012-6-15 19:42 上传
下载附件 (2.76 KB)
再次强调，我们的目标是找出一组参数，使整体的估计值最接近实际值。换句话说，就是：已知，求使I最小的。直接求有困难，要利用，不废话吗？这就是我们引入abc的原因啊。所以不用犹豫了，动用高数中的偏导吧，用I分别对求偏导，然后通通都要是0，这是I最小的必要条件。于是得到下面7条方程，对于，都是线性方程，组合起来就是一组7*7的齐次线性方程组。 http://cache.amobbs.com/new2012/forum/201206/15/194211g8ydcee2g4dgmycz.png.thumb.jpg 2012-6-15 19:42 上传
下载附件 (67.76 KB)
仔细观察上面的方程，很有规律。其实可以写成更简洁（传说中的数学的优雅啊）的形式。引入列向量v和pn，v为变量组，pn为系数组，单个偏差可以写成它们的点积。
http://cache.amobbs.com/new2012/forum/201206/15/194211sdp3mjr0n9pjj8dd.png 2012-6-15 19:42 上传
下载附件 (13.33 KB)
神奇的一刻到了，方程组可以用一条式表示！再经过一些变换（向量点积交换，证明不出，归纳出来的。），得到终极的矩阵形式。而且矩阵A相当利于程序实现，算法复杂度甚至是完美的“online”！
http://cache.amobbs.com/new2012/forum/201206/15/194212kop6ciroo55vik56.png 2012-6-15 19:42 上传
下载附件 (12.72 KB)
http://cache.amobbs.com/new2012/forum/201206/15/203155iq8qqbbfmyef6flq.png 2012-6-15 20:31 上传
下载附件 (5.05 KB)
呵呵，一不小心，把最小二乘法的一般形式推导出来了。
至此，问题归结到线性方程组的求解，理论上已经没有任何障碍（那是当然的，都已经用了上百年了）。
[*]编写软件 先梳理一下思路。现在有一个原始的RCD格式的数据，首先要提取出，然后用算出向量pn，再用pn转置自乘，累加成矩阵A，接着求以A为系数矩阵的齐次线性方程组，算出，这样就可以得到最佳的了。第一步需要了解RCD文件的结构，那是做黑匣的时候自己设计的，没问题；然后加加减减得到A，这个也不难；以前混ACM的时候，对着《线性代数》写过解线性方程组的库，解方程组小意思；最后再次加加减减，就出来啦。关键代码如下：
输入数据循环。
http://cache.amobbs.com/new2012/forum/201206/15/194214ko7akdgudohoooaa.png.thumb.jpg 2012-6-15 19:42 上传
下载附件 (65.43 KB)
累加生成矩阵。
http://cache.amobbs.com/new2012/forum/201206/15/194215qcq1yv0a8yql1qna.png.thumb.jpg 2012-6-15 19:42 上传
下载附件 (52.65 KB)
解方程，vp就是啦。
http://cache.amobbs.com/new2012/forum/201206/15/194217rfvfxhhhx9fhlzxl.png.thumb.jpg 2012-6-15 19:42 上传
下载附件 (67.81 KB)
转。
http://cache.amobbs.com/new2012/forum/201206/15/194218n0vvbnm2c2r7rbuo.png 2012-6-15 19:42 上传
下载附件 (16.39 KB)
[*]校验结果
一共14339组数据，左边是原始采样值，中间是标定后的加速度，右边是估计值与实际值的误差。
重力加速度取广州的9.7883m/s²，大多数误差在0.5%以下，结果还算满意。
http://cache.amobbs.com/new2012/forum/201206/15/194159pccuczuyczmeuyym.png.thumb.jpg 2012-6-15 19:41 上传
下载附件 (67.33 KB)
后来试过周期性忽略某些采样点，得出不同的参数，发现参数只有3～4位有效数字。
顶 顶 这个必须顶 顶 顶 顶 这个必须顶 顶
页:
[1]