kalman滤波和数字低通滤波

Kalman滤波和数字滤波

一、kalman滤波

卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。其他的就不介绍了。

公式简介

卡尔曼滤波主要是由5个经典公式组成：

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)的协方差还没更新。我们用P表示协方差：

P(k|k-1)=A P(k-1|k-1) A’+Q (2)

式(2)中，P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差，A’表示A的转置矩阵，Q是系统过程的协方差。式子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)的协方差：P(k|k)=（I-Kg(k) H）P(k|k-1) (5)

其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。

参数整定

卡尔曼滤波参数的调整：其参数有三个，P0是初始化最优角度估计的协方差（初始化最优角度估计可设为零），它是一个初值。Q是预测值的协方差，R是测量值的协方差。对Q 和R的设定只需记住，Q/(Q+R)的值就是卡尔曼增益的收敛值，比如其值为0.2，那么卡尔曼增益会向0.2收敛（对于0.2的含义解释一下，比如预测角度值是5度，角度测量值是10度，那么最优化角度为：5+0.2*（10-5）=6。从这里可以看出，卡尔曼增益越小，说明预测

值越可靠，最优化角度越接近预测值；相反的，卡尔曼增益越大，说明测量值越可靠，最优化角度越接近测量值）。P/(Q+R)反映收敛的快慢程度，该值设定越小，收敛越快，该值越大，收敛越慢（这里的P是指初始最优角度值的协方差），因为卡尔曼增益收敛总的来说是很快的，所以该值设定大一点或小一点都没什么关系，并且卡尔曼滤波器对于预测系统的要求并不

实现框图

卡尔曼滤波参数的调整：其参数有三个，P0是初始化最优角度估计的协方差（初始化最优角度估计可设为零），它是一个初值。Q是预测值的协方差，R是测量值的协方差。对Q 和R的设定只需记住，Q/(Q+R)的值就是卡尔曼增益的收敛值，比如其值为0.2，那么卡尔曼增益会向0.2收敛（对于0.2的含义解释一下，比如预测角度值是5度，角度测量值是10度，那么最优化角度为：5+0.2*（10-5）=6。从这里可以看出，卡尔曼增益越小，说明预测值越可靠，最优化角度越接近预测值；相反的，卡尔曼增益越大，说明测量值越可靠，最优化角度越接近测量值）。P/(Q+R)反映收敛的快慢程度，该值设定越小，收敛越快，该值越大，收敛越慢（这里的P是指初始最优角度值的协方差），因为卡尔曼增益收敛总的来说是很快的，所以该值设定大一点或小一点都没什么关系，并且卡尔曼滤波器对于预测系统的要求并不高。

Q，R的确定

测量噪声的协方差 R ，此为猜测值，因测量系统而定。系统噪声 Q ，也是猜测值，预测模型越不准，Q 值应越大，预测模型越精确，Q值也越小。在系统中为简便，Q、R常取定值。在系统归一化单位之后，可以确知测量模型和预测模型的取值范围，进而也就可以大致知道Q、R的取值，然后在试验中反复调试，观测系统输出，寻求最佳值。

二、数字滤波

数字滤波器和Matab

数字滤波器的功能是对输入离散信号的数字代码进行运算处理，以达到改变信号频谱的目的。离散傅里叶变换可以将抽样数字信号从时域转到频域，包括低通、高通、带通、带阻和全通等类型。

Matlab的信号处理工具箱的基本组成就是滤波器的设计与实现以及谱分析。工具箱提供了丰富而简便的设计、实现FIR和IIR的方法，使原来繁琐的程序设计简化成函数调用，特别是滤波器的表达方式和形式之间的相互转换显得十分简便，为滤波器的设计和实现开辟了一片广阔的天地。

matlab常用函数

（1）[N,wc]=buttord(wp，ws，wp，ws)

用于计算巴特沃斯数字滤波器的阶数N和3dB截止频率wc。调用参数wp，ws分别为数字滤波器的通带、阻带截止频率的归一化值，要求：0≤wp≤1，0≤ws≤1。1表示数字频率pi。wp，ws分别为通带最大衰减和组带最小衰减（dB）。当ws≤wp时，为高通滤波器；当wp和ws为二元矢量时，为带通或带阻滤波器，这时wc也是二元向量。N，wc作为butter函数的调用参数。

（2）[b，a]=butter（N，wc，‘ftype’）

计算N阶巴特沃斯数字滤波器系统函数分子、分母多项式的系数向量b、a。调用参数N和wc分别为巴特沃斯数字滤波器的阶数和3dB截止频率的归一化值（关于pi归一化），一般是调用buttord（1）格式计算N和wc。

(3)FFT 功能：一维快速傅立叶变换(FFT)。格式：y=fft(x)；y=fft(x，n)。

说明：fft函数用于计算矢量或矩阵的离散傅立叶变换。y=fft(x)利用肿算法计算矢量x的离散傅立叶变换，当x为矩阵时，y为矩阵x每一列的FFT。当x长度为2的整数次幂时，fft采用基2FFT算法，否则采用稍慢的混合基算法。y=fft(x，n)采用n点FFT。当x长度小于n时，fft函数自动在x尾部补零，以构成n点数列；当x长度大于n时，fft截取x 的前面n点数据进行FFT。

(4)IFFT 功能：一维逆快速傅立叶变换(IFFT)

格式：y=ifft(x)；y=ifft(x，n)。

(5) filter 一维数字滤波器

Y = filter(B,A,X) ，输入X为滤波前序列，Y为滤波结果序列，B/A 提供滤波器系数，B 为分子， A为分母。整个滤波过程是通过下面差分方程实现的： a(1)*y(n) = b(1)*x(n) +b(2)*x(n-1) + ... + b(nb+1)*x(n-nb) - a(2)*y(n-1) - ... - a(na+1)*y(n-na) [Y,Zf] = filter(B,A,X,Zi)，输入X为滤波前序列，Y为滤波结果序列，B/A 提供滤波器系数，B为分子， A为分母。

(6)fir1是用窗函数法设计线性相位RIRDF的工具箱函数，以实现线性相位FIRDF的标准窗函数法设计。

hn=fir1（M，wc，window），可以指定窗函数向量window。默认为哈明窗。例如，