对卡尔曼滤波的简单解释.docVIP

对卡尔曼滤波的简单解释先给出一个网上的例子：假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。 好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。 假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。 由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。 现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。 就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。 ---------------------------------- 对于四轴飞行器的倾角测量，跟上面的例子类似。首先可以由陀螺仪计算出一个倾角，即倾角预测值xhat_est = A * xhat + B * u 其次可以由加速度计算出一个倾角测量值y，究竟应该相信哪个多一点呢？这就需要卡尔曼增益K来决定了， 即更新的倾角xhat = xhat_est + K * Inn; 其中 Inn = y - c * xhat; 个人认为Inn应该=y-c*xhat_est才对，不知道是不是理解错了，有明白的高手请指点一下。  #include avr/io.h #include avr/iom16.h #include avr/interrupt.h #include math.h #include stdlib.h #include avr/pgmspace.h//----------------------------------- #define RXB8 1 #define TXB8 0 #define UPE 2 #define OVR 3 #define FE 4 #define UDRE 5 #define RXC 7 #define FRAMING_ERROR (1FE) #define PARITY_ERROR (1UPE) #define DATA_OVERRUN (1OVR) #define DATA_REGISTER_EMPTY (1UDRE) #define RX_COMPLETE (1RXC) //----------------------------------------- volatile unsigned int datadc; volatile unsigned char flagt0,flagt1,channel,data,adc_ready,ch0,ch1,ch2,ch3,ch4,ch5; volatile float gyro_x,gyro_y,accel_x,accel_y,accel_z,roll,pitch,roll_