卡尔曼滤波简单解释
来源:互联网 发布:5g 知乎 编辑:程序博客网 时间:2024/05/22 09:50
卡尔曼滤波:以陀螺仪测量的角速度作为预测值的控制量,加速度传感器测量的角度作为观测值。下面程序中angle_m为测量角度,gyro_m为测量角速度,gyro_m*dt为控制量。
以下程序是按卡尔曼滤波的五个公式来编写的。
X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
P(k|k)=(I-Kg(k) H)P(k|k-1) ……… (5)
对于单输入单输出系统,A、B、H、I不为矩阵且值都为1。
卡尔曼滤波参数的调整:其参数有三个,p0是初始化最优角度估计的协方差(初始化最优角度估计可设为零),它是一个初值。Q是预测值的协方差,R是测量值的协方差。对Q和R的设定只需记住,Q/(Q+R)的值就是卡尔曼增益的收敛值,比如其值为0.2,那么卡尔曼增益会向0.2收敛(对于0.2的含义解释一下,比如预测角度值是5度,角度测量值是10度,那么最优化角度为:5+0.2*(10-5)=6。从这里可以看出,卡尔曼增益越小,说明预测值越可靠,最优化角度越接近预测值;相反的,卡尔曼增益越大,说明测量值越可靠,最优化角度越接近测量值)。p0/(Q+R)反映收敛的快慢程度,该值设定越小,收敛越快,该值越大,收敛越慢(这里的p0是指初始最优角度值的协方差),因为卡尔曼增益收敛总的来说是很快的,所以该值设定大一点或小一点都没什么关系。
注:以下程序只用于说明算法,存在语法错误,初始的参数也是随意给定的。
x=0; /* 最优角度初值*/
p=1; /* 最优角度对应协方差初值*/
dt=0.02;
Q=0.0025;
R=0.25;
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{
x=x+ gyro_m*dt; 等号右边的x表示上一次最优角度值,等号左边的x表示这一次的角度的预测值
p=p+Q; 等号右边的p表示上一次最优角度值的协方差,等号左边的p表示这一次的角度预测值的协方差
k=p/(p+R); k值为卡尔曼增益(k值每次计算都不一样,它会越来越趋近于Q/(Q+R)这个收敛值)
x=x+k*( angle_m-x); 等号左边的x表示根据预测值和测量值计算出来的这一次的最优角度值(从这里可以看出,k越大,等号左边的最优值x与等号右边的测量值angle_m越接近;k越小,等号左边的最优值x与等号右边的预测值x越接近;)
p=(1-k)*p; 等号左边的p表示这一次最优角度值的协方差
}
从上面的程序可以看出,卡尔曼滤波是一个递推过程,初始的最优角度值可设为x=0,初始最优角度值的协方差p一定不能设为零,dt是采样周期,Q 与R可共同决定卡尔曼增益收敛的大小。
- 卡尔曼滤波简单解释
- 简单卡尔曼滤波
- 通俗解释卡尔曼滤波
- 通俗解释卡尔曼滤波
- 卡尔曼滤波、粒子滤波【通俗解释】
- 简单卡尔曼滤波【转】
- 卡尔曼滤波程序matlab 解释
- 卡尔曼滤波和粒子滤波最直白的解释
- 卡尔曼滤波的原理简单说明
- 【Kalman】卡尔曼滤波Matlab简单实现
- 卡尔曼滤波 – Kalman Filter (通俗的解释)
- 卡尔曼滤波 – Kalman Filter (通俗的解释)
- 如何通俗并尽可能详细解释卡尔曼滤波?
- 关于卡尔曼滤波本质解释和公式推到
- 卡尔曼滤波形象解释及c++实现
- 卡尔曼滤波(Kalman Filter)的通俗解释
- 扩展卡尔曼滤波+卡尔曼滤波
- 算法笔记-卡尔曼滤波器简单解释
- Python 学习入门(32)—— xrange/range
- getResourceAsStream()
- 每天一个linux命令,包含最常用的几十个命令的详细使用,让您快速掌握Linux!
- java接口介绍
- Cocos2d-x 中C++调用java(jni)
- 卡尔曼滤波简单解释
- linux 编译命令备注
- 2013-12-12 17:10:48
- 类名.class的含义
- IOS 之 通讯录
- A New Vulnerability in the Android Framework: Fragment Injection
- 【九度】题目1505:两个链表的第一个公共结点
- wrong @@SERVERNAME
- 运营商路由器和交换机市场今年有望达202亿美元