卡尔曼滤波程序（Calman filtering procedure）.docVIP

卡尔曼滤波程序（Calman filtering procedure） *************************************************************************** / / / *卡尔曼 / *一维卡尔曼滤波算法，使用测斜仪和陀螺仪* 作者：黄富智/ * * / / *版本：1 * 日期：30.05.2003 * / / * / *改编自Trammel Hudson（hudson@）* / ------------------------------- / * * / *编译程序：* / * Linux * / * gcc68 C xxxxxx。C（创建对象文件）* / / * gcc68 O xxxxxx.hex xxxxxx。O PPWA。O * / *上载数据：* * *************************************************************************** / / *在本版本中：* *************************************************************************** / / *这是一个免费软件；你可以重新分配它和/或修改**。 *根据已发布的GNU通用公共许可证条款 / *由自由软件基金会；或许可证的第2版，* *或（在您的选择）任何稍后版本。* / / ** *本代码是分发的，希望它有用。 *但没有任何保证；甚至不包括*的默示保证。 / *适销性或针对特定用途的。见** / * GNU通用公共许可证以获得更多详情。* / / ** / *您应该已经收到GNU通用公共许可证的副本* *随自动驾驶仪；如果没有，请写入自由软件* / *基金会，59圣殿，330号套房，波士顿，** / * * / MA02111-1307 USA *************************************************************************** / / #包括数学。” #包括“眼点。” / * *的状态与速率陀螺测量每20ms的更新 *如果以不同的速率更新，则更改此值。 * / 静态常量浮点dt = 0.02； / * *协方差矩阵。这是在每一步更新到 *确定传感器跟踪实际状态的程度。 * / 静态浮动[ 2 ] ] 2 = { 1, 0 }， { 0, 1 } }； / * *我们的两种状态，角度和陀螺偏置，作为计算的副产品。 *角，我们也有一个不偏不倚的角速率可用。 *对模块的用户只读。 * / 浮动角； 浮q_bias； 浮动费率； / * * R代表测量噪声协方差R = E [时]。 *在这种情况下，它是一个1x1矩阵表示，我们期望 *测斜仪的0.1弧度抖动。 *在这种情况下，V = 0.1个1x1矩阵 * / 静态常量浮r_angle = 0.001； / * * Q是一个2x2矩阵表示的过程噪声协方差。 *在这种情况下，它表明我们多么信任测斜仪。 *相对于陀螺仪。 * / 静态常量浮q_angle = 0.001； 静态常量浮q_gyro = 0.0015； / * * state_update叫每个DT带偏见的陀螺测量 *由模块的用户。它更新当前的角度和 *速率估计。 * *螺距陀螺测量应缩放成实际单位，但 *不需要任何偏移去除。过滤器将跟踪偏置。 * * = [ 0 - 1 ] * [ 0 0 ] * / 无效stateupdate（const浮q_m）{ 浮Q； 浮PDOT [ 4 ]； / * Unbias * /我们的陀螺 Q = q_m - q_bias； / * *计算协方差矩阵的导数 *（方程22-1） *一个* PDOT = P + P + Q * * * / PDOT [ 0 ] = q_angle P [ 0 ] [ 1 ] P [ 1 ] [ 0 ]；/ * * / 0 PDOT [ 1 ] = P [ 1 ] [ 1 ]；/ * * / 0 PDOT [ 2 ] = P [ 1 ] [ 1 ]；/ * * / 1,0 PDOT [ 3 ] = q_gyro；/ * * / 1 / *存储我们的无偏估计* /陀螺 速率＝q； / * *更新我们的角度估计 *角+ = angle_dot * DT * + =（陀螺gyro_bias）* DT * = q * dt * / 角度= q＝dt； *更新协方差矩阵* P [ 0 ] [ 0 ] [ 0 ] * + = PDOT DT； P [ 0 ] [ 1 ] [ 1 ] * + = PDOT DT； P [ 1 ] [ 0 ]