惯性数据测量——卡尔曼滤波.docVIP

惯性数据测量——卡尔曼滤波 介绍 对于大多数人来说，卡尔曼滤波很难理解。的确，他奇迹般地解决了其他方法很难解决的问题。但是注意，卡尔曼滤波并不是解决你所有问题万能的良方。 这篇文章将阐述卡尔曼滤波的使用。我们将使用多的实际方法而避免那些很难理解的繁琐理论，因为大多数人仅仅将其用于MAV/UAV（微型飞行器/无人机）应用上，我将试图将它形象具体化。 必须确保你已经知道了怎样使用加速度计和陀螺仪来进行数据采集，另外，一些基础的代数知识也是必要的。 基本操作 卡尔曼滤波是一种链形滤波器（累接滤波器），他需要两个东西： 首先，你将需要几种输入（一路或更多的数据源），你可以仅仅使用线性计算将他们转换为你预期的输出。换句话说，在我们的问题中，我们需要建立一个线性模型。 其次，你需要另一路输入。这一路是期望数据中的实际外界数据值，或者是它的近似。 每一次迭代，卡尔曼滤波器都将微小地改变线性模型中的变量，因此线性模型的输出将于第二次输入接近。 简化模型 显然，两路输入包含陀螺仪和加速度计的数据。使用陀螺仪数据模型看起来像这样： 第一个公式展示了一个线性模型的一般形式。我们需要在B矩阵中“填入”A矩阵，然后选择一个状态x。变量_u_k代表输入。对我们来说，这就是陀螺仪的数据。记住，怎样去积分？我们只是对归一化的数据进行求和： alpha k = alpha k-1 + (_u_ k – bias) 我们需要再两个测量(_dt_)之间加入时间参数，因为我们要处理角速度(degrees/s):? alpha k = alpha k-1 + (_u_ k – bias) * dt 重写如下： alpha k = alpha k-1 – bias * dt + u k * dt 上式在矩阵乘法中我将用到。注意bias保持恒定！在个别陀螺仪中，我们看到了bias的漂移。接下来就是卡尔曼的神奇之处了：滤波器通过将加速度计的输出和结果进行比较调整每次迭代的bias。真是棒极了！。 包装总结： 现在，我们需要bits and bolts来做这件神奇的事了。这些用到了矩阵代数和统计学中的公式。没有必要立刻就知晓他们的细节。如下： u= measurement1??????? 读取上一次测量的数据 x= A ? x + B ? u??????? 更新模型中的状态x y= measurement2??????? 读第二次测量的数据. 数据来源于加速度计，将对角度进行计算 Inn= y – C ? x??????? 计算先前模型中的数据和第二次测量数据的差值，This is called the innovation s= C ? P ? C’ + Sz??????? 计算协方差 K= A ? P ? C’ ? inv(_s_)??????? 计算卡尔曼增益 x= x + K ? Inn??????? 修正先前状态值 P= A ? P ? A’ – K ? C ? P ? A’ + Sw??????? 计算先前误差的协方差 矩阵C用于提取状态矩阵中的输出。对于我们而言，它是(1 0)’ : alpha = C ? x Sz是测量过程的噪声协方差：Sz = E(zk zkT) 在我们的例子中，这是加速度计数据我们期望的抖动程度。 Sw 地方过程噪声协方差矩阵（这里是2×2矩阵）：Sw = E(x ? xT) 这样：Sw = E( [alpha bias]’ ? [alpha bias] ) 因为仅仅只有用到Sw矩阵的对角元素，因此我们只需要知道E(alpha2) 和E(bias2)，他们是第二个时刻的值。为了计算这些值，我们需要查看我们建立的模型：alpha中的噪声来自于陀螺仪，同时乘以dt2这样：E(alpha2) = E(u2)? dt2。 这些因素取决于你使用的传感器，你需要通过做实验把他们计算出来。在autopilot/rotomotion的卡尔曼滤波的源代码中，他们使用以下的常量： E(alpha2) = 0.001 E(bias2) = 0.003 Sz = 0.3 (radians = 17.2 degrees) 以下为英文原文：  HYPERLINK http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data Kalman filtering of IMU data Introduction To many of us, kalman filtering is something like the holy grail. Indeed, it miraculously solves some problems which are otherwise hard to get a hold on. B