|
|
楼主 |
发表于 2010-4-14 00:38:22
|
显示全部楼层
我认为那个模型没有问题。
我们构建一个系统状态量： x = [ angle, bias ]'，表示某一轴的角度值及其偏差。
其系统状态方程可以表示为:
* x = [ angle, bias ]'
*
* = F x + B u
*
* = [ 1 -dt, 0 1 ] [ angle, bias ] + [ dt, 0 ] [ dotAngle 0 ]
*
* => angle = angle + dt (dotAngle - bias)
* bias = bias
dotAngle为角速率。可以看出，下一时刻的角度等于当前角度加上去偏差的角速度与两次时间间隔的乘积。这是没有错的。
而下一时刻的角度偏差用这一时刻的偏差来更新也是没有错的。
所以，我认为系统模型没有什么问题。
另外我要讲一点，kalman滤波器并不是万能的，也并不是说，来了一阵持续时间很长的妖风，Kalman仍然可以有效的工作。
Kalman是建立在系统模型准确的情况下才是最优滤波器，而本系统模型在出生之时就被定义了误差是白噪声，均值为0。
如果妖风来的太久，显然就不是均值为0了。这样，滤波出来的值会慢慢的向测量值逼近。
而Kamlan在本例中最本质的还是信息融合。它把陀螺的数据应用于系统的状态方程里，这个更新叫做系统状态更新。
这就是上面的系统状态方程所做的事。
与此同时，它还更新了系统的预测方差矩阵P
* P = F P transpose(F) + Q
*
* = [ 1 -dt, 0 1 ] * P * [ 1 0, -dt 1 ] + Q
*
* P(0,0) = P(0,0) - dt * ( P(1,0) + P(0,1) ) + dt* P(1,1) + Q(0,0)
* P(0,1) = P(0,1) - dt * P(1,1) + Q(0,1)
* P(1,0) = P(1,0) - dt * P(1,1) + Q(1,0)
* P(1,1) = P(1,1) + Q(1,1)
这个时候，X所预测的值是通过状态方程估计出来的，到底准不准确还不知道。所以，在此，我们引入了一个测量值，
来检测我们的测量是否准确。这个测量值我们用加速度来得到：atan2(ac_x, ac_z)或atan2(ac_y, ac_z)
但我们都知道，加表在运动下的测量值是不准的。这个时候，我们就要用一些先验的知识来对两个值要融合的权值做
一个计算。这个先验知识就是R：测量噪声协方差和Q：系统状态噪声协方差。
令y为测量值与系统状态估计值之间的误差：
y = z - H x （H为测量值到观测量之间的传递矩阵）
而K为测量值对状态量的修正增益：
* S = H P transpose(H) + R
* = [ 1 0 ] P [ 1, 0 ] + R
* = P(0,0) + R
*
* K = P transpose(H) S^-1
* = [ P(0,0), P(1,0) ] / S
对X修正：
* x = x + K y
然后再计算系统的误差协方差矩阵：
* P = (I - K H) P
*
* = ( [ 1 0, [ K(0),
* 0 1 ] - K(1) ] * [ 1 0 ] ) P
*
* = [ P(0,0)-P(0,0)*K(0) P(0,1)-P(0,1)*K(0),
* P(1,0)-P(0,0)*K(1) P(1,1)-P(0,1)*K(1) ]
整个过程中可以看出来，对X的修正增益K与R和Q值都有关系。如果R大Q小，就是说，状态估计值比测量值要可靠，这时，所得出的结果
就是更接近估计值。如果R小Q大，这时，计算出来的结果就会更接近测量值。
实际上，在飞行器的运行中，因为振动和环境影响，加表的值可信度是很低的，而陀螺受振动的影响要小的多。所以，一般Q值会选的比
较小，而R值会选的比较大。但振动以及妖风的影响不可能是长时间向某一个方向来的，总的来说，加表测出来的倾角均值为0.这样，就
会给陀螺提供一个基准，因为陀螺是有积累误差的，这个误差随着时间的推移而变大发散。而加表长时间的基准就是为了对陀螺的积累误差
做一个修正。这样来看的话，可以看做对加表做了个截止频率很低的低通，用以保证基准；而陀螺做了一个截止频率很低的高通，用以保证
响应度。
其实这些工作也可以用互补滤波器来做，德国人的四轴就是用互补来做的。效果也可以。
这些天太忙，有项目要验收，没有来得及回复。抱歉。另外，我也用互补滤波器做了一个，感觉比用这个Kalman还要OK一点。过两天再传
视频。
这是我自己的理解，因为我本身不是控制出身的，这块肯定有不当之处，欢迎拍砖！！ |
|
[复制链接]
发表于 2008-8-13 00:22:58
