卡尔曼滤波 平衡车

kalman滤波器分类：CV相关2012-03-19 00:26 178人阅读评论(2) 收藏举报卡尔曼是匈牙利当代著名数学家，Kalman滤波器源自于他的博士毕业论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。

卡尔曼滤波器是一个最优化自回归数据处理方法，它是一个时域滤波器，是通过对时域上包含噪声的有限测量数据，计算出最接近实际值的方法。

这里说它是一种递归的估计，是指只要获得上一时刻的状态值以及当前的状态观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。

既然是一个时域滤波器，也就无需像低通滤波器那样，需要在频域进行滤波器设计而需要再转换到时域来实现。

卡尔曼滤波器的典型应用是从一组包含噪声的对无物体位置的观察序列中预测出物体的位置坐标和速度，它的核心内容是5个计算公式，然而，仅仅给出5个计算公式显得过于抽象，不便于理解。

这里，为了更加形象、更容易地理解卡尔曼滤波器，我给出网上摘取的一个例子来一步一步探索卡尔曼滤波器的原理。

一、举例说明卡尔曼滤波器的原理（该例子转载自网络）假设被研究对象是某个房间内的温度（当然，这里假设该房间内各处的温度是相等的，房间内不存在空调、风扇、暖气等局部发热或散热的设备）。

根据生活经验，你认为这个房间内的温度是恒定的。

即，下一时刻的温度等于当前时刻的温度。

但是，你的生活经验并不是绝对的准确，所以，你确信自己对房间内温度的估计存在上下几度的偏差。

这个偏差在这里假设是高斯白噪声的，即：偏差跟时间没有关系而且是服从高斯分布的。

另外，在房间内放置一个温度计，用于测量房间内的温度。

但是，由于制造工艺等因素，温度计的测量值也不是绝对准确的，即：温度计的测量值与真实值之间也存在一定的偏差，这个偏差也假设是高斯白噪声的。

那么，对于任意时刻，我们可以得到该房间的两个温度值：你根据生活经验得到的估计值（系统的预测值）和温度计的测量值（系统的测量值）。

下面我们用这两个值结合他们各自的噪声来估算出房间的实际温度值。

假如我们要估算k时刻的实际温度值。

首先你要根据k-1时刻的温度值，来预测k时刻的温度。

（1）因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度。

（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）（2）然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。

究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的协方差来判断。

因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。

可以看出，因为温度计的协方差比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。

现在我们已经得到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）。

就是这样，卡尔曼滤波器就不断的把协方差递归，从而估算出最优的温度值。

他运行的很快，而且它只保留了上一时刻的协方差。

上面的Kg，就是卡尔曼增益（Kalman Gain）。

他可以随不同的时刻而改变他自己的值。

二、卡尔曼滤波器的数学模型讨论卡尔曼滤波器就不得不提离散控制过程的系统。

对于一个离散控制过程的系统而言，可用一个线性随机微分方程来描述：X(k)=A X(k-1)+B U(k)+W(k)系统的测量值：Z(k)=H X(k)+V(k)上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。

A和B是系统参数，对于多模型系统，他们为矩阵。

Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。

W(k)和V(k)分别表示过程和测量的噪声。

他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance分别是Q，R（这里我们假设他们不随系统状态变化而变化）。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。

下面我们来用他们结合他们的covariances来估算系统的最优化输出（类似上一节那个温度的例子）。

首先我们要利用系统的过程模型，来预测下一状态的系统。

假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：X(k|k-1)=A X(k-1|k-1)+B U(k) (1)式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。

到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance还没更新。

我们用P表示covariance：P(k|k-1)=A P(k-1|k-1) A’+Q (2)式(2)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的covariance，A’表示A的转置矩阵，Q是系统过程的covariance。

式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。

现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。

结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) (3)其中Kg为卡尔曼增益(Kalman Gain)：Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) (4)到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。

但是为了要令卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：P(k|k)=（I-Kg(k) H）P(k|k-1) (5)根据这5个公式，可以很容易的实现计算机的程序。

其实我也是一知半解，给你推荐篇文章卡尔曼滤波介绍中文版ourdev_611837OCLGOY.pdf(文件大小:540K)(原文件名:kalman_intro_chinese.pdf)卡尔曼滤波介绍英文版ourdev_611838G0F5Z1.pdf(文件大小:173K)(原文件名:kalman_intro.pdf)文章从头开始看，看完离散卡尔曼滤波那一节，就会理解很多。

后面扩展卡尔曼我看不懂，最后面举了个例子，好好看下。

卡尔曼C语言ourdev_611839GC1Z3U.txt(文件大小:7K)(原文件名:kalman.txt)再看下这个程序。

和我用的结构差不多的（我的是抄zlstone的，他貌似也是复制的，呵呵，最难的就是卡尔曼）。

下面是我的：void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure{angle+=(gyro_m-q_bias) * dt;//先验估计Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分Pdot[1]=- P[1][1];Pdot[2]=- P[1][1];Pdot[3]=Q_gyro;P[0][0] += Pdot[0] * dt;// Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差P[0][1] += Pdot[1] * dt;P[1][0] += Pdot[2] * dt;P[1][1] += Pdot[3] * dt;angle_err = angle_m - angle;//zk-先验估计PCt_0 = C_0 * P[0][0];PCt_1 = C_0 * P[1][0];E = R_angle + C_0 * PCt_0;K_0 = PCt_0 / E;//KkK_1 = PCt_1 / E;t_0 = PCt_0;t_1 = C_0 * P[0][1];P[0][0] -= K_0 * t_0;//后验估计误差协方差P[0][1] -= K_0 * t_1;P[1][0] -= K_1 * t_0;P[1][1] -= K_1 * t_1;angle += K_0 * angle_err;//后验估计q_bias += K_1 * angle_err;//后验估计angle_dot = gyro_m-q_bias;//输出值（后验估计）的微分 = 角速度}我只能帮这些了，我的方向是运动控制方面，KALMAN貌似在信号分析相关专业会学的，所以我也没细看了。

上述程序我是直接复制过来的，只改了采样周期，剩下一点没动（还加了点注释，呵呵）。

有关KALMAN 的详细解释一般都是外国网站的，中国的都是扯淡。

我做这个车的目的就是想搞下PID，怎么获取角度是次要的东西。

OK：其实控制很简单，利用定时器产生10ms中断，每次中断进行如下工作：1：AD采样加速度传感器和陀螺仪，然后卡尔曼滤波得出角度与角速度；（滤波模块借鉴老外的）（互补滤波效果感觉不是很好）；2：计算车轮的速度，积分得出位置；3：利用PD算法得出PWM值=K1*angle + K2*angle_dot + K3*speed + K4 * position；4：前进后退给定参考速度计算即可；注：1：增量式轮速传感器A相中断，读取B相电位判断前进还是后退，在10ms的时间内累加，计算车轮的速度；2：由于10ms累加的轮速信号不多，直接计算车体会发抖，所以增加了低通滤波，解决问题；3：我在外面增加4个电位器可以手动调节4个K值，方便调试；以上；关于卡尔曼滤波的解释。

正在看，大家看看。

1．什么是卡尔曼滤波器（What is the Kalman Filter?）在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。

跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。