卡尔曼滤波器及matlab代码

信息融合大作业维纳最速卜降法滤波器，卡尔曼滤波器设计及Matlab仿真时间：2010-12-6专业：信息工程班级：09030702学号：2007302171姓名：马志强1. 滤波问题浅谈估计器或滤波器这一术语通常用來称呼一个系统，设计这样的系统是为了从 含有噪声的数据中提取人们感兴趣的，接近规定质最的信息。由于这样一个宽 目标，估计理论应用于诸如通信、雷达、声纳、导航、地震学、生物医学工程、 金融工程等众多不同的领域。例如，考虑一个数字通信系统，其基本形式由发 射机、信道和接收机连接组成。发射机的作用是把数字源(例如计算机)产生的0、 1符号序列组成的消息信号变换成为适合于信道上传送的波形。而由丁符号间 干扰和噪声的存在，信道输出端收到的信号是含有噪声的或失真的发送信号。 接收机的作用是，操作接收信号并把原消息信号的一个可靠估值传递给系统输 出端的某个用户。随着通信系统复杂度的提高，对原消息信号的还原成为通信 系统中最为重要的环节，而噪声是接收端需要排除的最主要的干扰，人们也设 计出了针对各种不同条件应用的滤波器，其中最速下降算法是一种古老的最优 化技术，而卡尔曼滤波器随着应用条件的精简成为了普适性的高效滤波器。2. 维纳最速下降算法滤波器2.1最速下降算法的基本思想考虑一个代价函数丿(w),它是某个未知向量w的连续可微分函数。函数丿(w) 将W的元素映射为实数。这里，我们要寻找一个最优解使它满足如下条件 /(Wo) J(w)(2.1) 这也是无约束最优化的数学表示。特别适合于自适应滤波的一类无约束最优化算法基于局部迭代下降的算法: 从某一初始猜想w(0)出发，产生一系列权向frw(l),w(2),，使得代价函 数/(w)在其法的每一次迭代祁是下降的.即；(w(n+l)/(w(n)其中W(“)是权向研的过去值，而W(71+l)是其更新值我们希里算法最终收敛到最优值W。迭代下降的一种简单形式是最速下降 法，该方法是沿最速下降方向连续调整权向量。为方便起见，我们将梯度向量表示为173)=dw(2.2)因此，最速下降法可以表示为1w(n + 1) = w(n) 一-jU7(n)(23)其中n代表进程，“是正常数，称为步长参数,1/2因子的引入是为了数学上处 理方便。在从71到H+1的迭代中，权向量的调整量为15w(n) = w(ji + 1) w(n) = c/(n)乙(2.4)为了证明最速下降算法满足式(2.1),在w(町处进行一阶泰勒展开，得到J(w(n + 1) a J(w(n) + gH(n)6w(n)(2.5) 此式对于“较小时是成立的。在式(2.4)中设w为负值向量，因而梯度向験/也为 负值向量，所以使用埃尔米特转置。将式(2.4)用到式(2.5)中，得到丿3(/1 + 1)乂 丿3(勿)一扣 |g(n)|2此式表明当卩为正数时，/(w(n+l)/(w(n)o因此，随着71的增加，代价函 数丿5)减小，当71 = 8时，代价函数趋于最小值J8。2.2最速下降算法应用于维纳滤波器考虑一个横向滤波器，其抽头输入为u(n),u(n-l),.-,u(n-M + 1),对应 的抽头权值为w(n),wi(n),Wmt(h)。抽头输入是來自零均值、相关矩阵为R 的广义半稳随机过程的抽样值。除了这些输入外，滤波器还要一个期與响应d), 以便为最优滤波提供一个参考。在时刻71抽头输入向量表示为“(n),滤波器输 出端期望响应的估计值为d(n|/n),其中人是由抽头输u(n),u(n- l), -,u(n- M + 1)所张成的空间。空过比较期望响应d(n)及其估计值，可以得到一个估计 误差e(n),即e(n) = d(h) d(n|l/n) = d(n) wH(h)u(h)(2.6)这里wH(n)u(n)是抽头权向量w(m)与抽头输入向5：u(n)的内积。w(?i)可以进一 步表示为w(n) = w0(n)f wx(n)f , wM.x(n)r同样，抽头输入向:u(n)可表示为u(n) = u(n)fu(n I),-,u(n M + l)r如果抽头输入向暈U(n)和期望响应d(7l)是联合平稳的，此时均方误差或者 在时刻的代价函数丿(町是抽头权向量的二次函数，于是可以得到/(n) = oI - wH(ji)p - pHw(n) + wHn)Rw(n)(2 刀 其中，於为目标函数d(7l)的方差，p抽头输入向量(7!)与期望响应d(M)的互相 关向量，及R为抽头输入向S：u(n)的相关矩阵c从而梯度向星可以写为r/(n)=da0(ri) 可(n)0ai(n)4-+.dJW可(n)7 血15)=2p + 2Rw(n)dJW+可(n)0切-心)(2.8)dak(n) dbk(n)其中在列向量中字哄和弈分别是代价函数/(”对应第k个抽头权值叭(町的 实部H(M)和虚部/()的偏导数。对最速下降算法应用而言，假设式(2. 8)中相关矩阵R和互相关向量P己知，则对于给定的抽头权向量W(7i + 1)为w(n + 1) = w(n) + np Rw(ri)(2.9)它描述了为那滤波中最速下降法的数学表达式。3 卡尔曼滤波器3.1卡尔曼滤波器的基本思想卡尔曼滤波器是用状态空间概念描述共数学公式的，另外新颖的特点是，他的解递归运算，可以不加修改地应用于平稳和非平稳环境。尤其是，其状态的每一次更新估计都由前一次估计和新的输入数据计算得到，因此只需存储前 一次估计。除了不需要存储过去的所有观测数据外，卡尔曼滤波计算比直接根 据滤波过程中每一步所有过去数据进行估值的方法都更加有效。Vi(n)x(n +1)x(n)y(n)ouo 二 尸二o巾)图3.1线性动态离散时间系统的信号流图表示“状态”的概念是这种表示的基础。状态向量，简单地说状态，定义为数据 的最小集合，这组数据足以唯一地描述系统的自然动态行为。换句话说，状态由 预测系统未來特性时所素要的，与系统的过去行为有关的最少的数据组成。典型 地，比较有代表性的情况是，状态兀)是未知的。为了估计它，我们使用一组观 测数据，在途中用向量y(n)表示。y(/i)成为观测向屋或者简称观测值，并假设它 是N维的。在数学上，图3.1表示的信号流图隐含着一下两个方程：(1) 过程方程x(n + 1) = F(n 4- l,n)x(n) + vt(n)(3.1) 式中，MX1向量巧(町表示噪声过程，可建模为零均值的白噪声过程，且其 相关矩阵定义为h(n)v?(fc) = (n)鳥：(2) 测量方程y(n) = c (n)x(n) 4- v2(n)(3.2) 其中“小是已知的N X M测最矩阵。N X 1向ftv2(n)称为测量噪声，建模 为零均值的白噪声过程，其相关矩阵为E”2(3她) = 肿)鳥:(3.3) 测量方程(3. 2)确立了可测系统输岀y(町与状态x(n)之间的关系，如图 3. 1所示。3.2新息过程为了求解卡尔曼滤波问题，我们将应用基于新息过程的方法。根据之前所述, 用向RyCnlyn-x)表示n = 1时刻到八-1时刻所有观测数据过去值给定的情况下, 你时刻观测数据y(n)的最小均方估计。过去的值用观测值y(l)(2)，(n- 1) 表示，他们张成的向量空间用表示。从而可以定义新息过程如下：ot(n) =y(n)-y(n|yn_i)(3.4) 其中M X 1向Sa(n)表示观测数据y(n)的新息。3.3应用新息过程进行状态估计下面，我们根据信息过程导出状态“)的最小均方估计。根据推导，这个估 计可以表示成为新息过程a(l),a(2),-,a(n)序列的线性组合，即nx(i|y)=W(k)k=l(3.5) 其中艮伙)忙“是一组待定的MXN矩阵。根据正交性原理，预测状态误差向量 与新息过程正交，即E(ifn)aH(m) = Ex(i) - xiynaH(m) = O(3.6) 将式(3.5)代入式(3.6),并利用新息过程的正交性质，即得(3.7) 因此，式(3.刀两边同时右乘逆矩阵R7(m),可得创(m)的表达式为Bj(m) = E x(0 aH(m) 1 (m)(3.8) 最后，将式(3.8)带入式(3.5),可得最小军方差估计n x(t|yn)=工 ExaHWR-k)aW k=ln-1 =乂 Ex(i)aH(QR7(/c)a(k) +Ejr(i)(rH(n)7r1(n)仇(n) k=l(3.9) 故对于i =71+1,有n-1x(n + l|yn)=工 Ex(n + 1)/伙)/T丄(k)a伙)k=l+ Ex(n + l)aH(n)J?1(n)a(n)(3.10) 然而，n + 1时刻的状态X(7l + 1)与71时刻的状态x(ti)的关系式由式可以推导出对 于0 k n,有Ex(n + l)aH(k) = EF(n + 1, n)x(n) + Vi(n)aH(k)=F(n + 1,砒引班砒卅仇)(3.11)其中a(k)只与观测数据丁(1),歹(2),丁伙)有关。因此可知，vi(n)与a(k)彼此正 交(其中0 kn)o利用式(3.11)以及当i = n时兒(i|yn)的计算公式,可将式(3.10)H-1乂 Ex(n + l)aH(k)K-x(k)a(k)k=l右边的求和项改写为n-1=F(n + l,n)工 Ex(n)aH(/)Zri(k)a伙) k=l=F(n + 1, n) x(n|yn_i)(3.12)为了进一步讨论，引入如下基本定义。3.4卡尔曼增益定义MXN矩阵G(n) = Ex(n + l)aH(n) R“(k)(3.13)其中Ex(n + 1)土(町是状态向量班71 + 1)和新息过程2(71)的互相关矩阵。利用 这一定义和式(3.12)的结果，可以将式(3.10)简单重写为x(n + l|yn) = F(n + l,n)x(n|yn_i)+ G(n)a(n)(3.11) 式(3.14)具有明确的物理意义。它标明：线性动态系统状态的最小均方估计 x(n+ 1|%)可以由前一个估计免(八叽J求得。为了表示对卡尔曼开创性贡献的 认可，将矩阵G(n)称为卡尔曼增益。现在剩下唯一要解决的问题是，怎样以一种便于计算的形式来表示卡尔曼增 益G(n)o为此，酋先将x(n 4-1)与仇H)乘积的期望表示为Ex(n + l)aH (n) = F(n+ 1, n) Ex(n)H(n, n l)CH(n)(3.12) 式中利用了状态班n)与噪声向v2(n)互不相关这一事实。其次，由于预测状态 误差向一 1)与估计x(n|yn_i)正交，因比无仇叽_与占,” 一 1)乘机的 期望为零。这样，用预测状态误差向量(n,n-l)代替相乘因子x(n),将不会引 起式(3.15)变化，故有Ex(n+ l)aH(n) = F(n + 1, n)Ee(n,n 一 1)eh(h,h- l)CH(n)(3.13) 由此，可将上式进一步变化为Ex(n 4- l)aH(n) = F(n+ l,n)K(n,n- l)CH(n)(3.14) 现在我们重新定义卡尔曼增益。为此，将式(3.17)代入式(3.13)得G(n) = F(n + l,n)K(n,n - l)CH(n) Rk)(3.15) 现在我们已经了解了卡尔曼滤波的整个过程利相应的参数设置，为了能够更为方 便利用计算机仿真实现，特将其中参数变量进行小结。卡尔曼变量和参数小结变童定义维数x(n)n时刻状态M x 1yW丹时刻状态值iVx 1F(n + l,n)从71时刻到71 + 1时刻的转移矩阵M xMC(n)n时刻的测量矩阵NxMQ心)过程噪声“100的相关矩阵M xM过程噪声巾00的相关矩阵NxN*(nlyn-i)给定观测值y(l),y(2),y(n -M x 11)在“时刻状态的预测佔计飢“ly给定观测值也)(2),(町在Mxln时刻状态的濾波估计G(n)n时刻卡尔曼增益矩阵M xNa(n)71时刻新息向fitNx 1R(n)新息向T.a(n)的相关矩阵NxNK(nt n 1)x(n|yn_x)|误基相关矩阵MxMx(n|yn)中课差相关矩阵M xM基于单步预测的卡尔曼滤波器的小结观测t=y(l),y(2), -,y(n- 1)转移矩阵=F(n + l,n)测呈矩阵=C(n)过程噪声的郴关矩阵=?!()测呈噪声的柑关矩阵=(?2(n)G(n) = F(n + l,n)K(n,n l)CH(n) C(n)K(n, n - l)Cw(n) + (?2(n)_1l&q2/Ryx 最大特征值hn=zeros(l,N);hn(:)=5;vg=O;Rxx=xcorr(l);Ryx=mi n( min(corrcoef(l, 1+rand n);echo offfor i=l:N-l;%vg=2*Rxx*hn(:,i)-2*Ryx;%hn(:,i+l)=hn(:,i)-l/2*q*vg;vg=2*Rxx*hn(i)-2*Ryx;hn(i+l)=hn(i)-l/2*q*vg;m(i)=l；endl=l:N-l;plot(t,hn(t),W,t,m(t),b)；5.3后联平滑滤波器的卡尔曼滤波器clearclc;N=300;CON = 5;x = zeros(l,N);x(l)=l；P = 10；Q = randn(lzN)#0.2;%ii 程噪声协方差R = randn(lzN);%观测噪声协方差y=R + CON;%加过程噪声的状态输出for k = 2 : NQI = cov(Q(l:k-l);%过程噪声协方差Q2 = cov(R(l:k-l);x(k) = x(k -1);%预估计k时刻状态变量的值p = p + Q1;%对应丁预估值的协方差kg 二 P / (p + Q2);%kalman gainx(k) = x(k) + kg * (y(k) - x(k);p = (1 - kg) p;endFilter_Wid = 10;smooth_res = zeros(l,N);kalmanp = zeros(l，N)；for i = Filter_Wid + 1: Ntempsum = 0;kalman_m = 0;for j = i - Filter_Wid : i -1tempsum = tempsum + y(j); kalma n_m = kalman_m+x(j);endkalman_p(i) = kalman_m/Filter_Wid;smooth_res(i) = tempsum / Filter_Wid;%平滑滤波 end% figure(l);% hist(y);t=l:N;figure(l);expValue = zeros(lzN);for i = 1: NexpValue(i) = CON;end plot(t/expValue/,r,/t/x/,g,/t/y,b,/t/smooth-reszlk,/t/kalman_pzlm,)； legendfruthVestimate/measure/smooth resulfjsmooth kalman)； %axis(0 N 0 30)xlabel(Sample time);ylabelCRoom Temperature);title(Smooth filter VS kalman filter1)；