根据matlab卡尔曼滤波算法仿真.pdfVIP

基于 Matlab 的卡尔曼滤波算法仿真 1. 卡尔曼滤波器原理 卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计 值和最近一个观察数据来估计信号的当前值。它是用状态方程和递推方法进行估计的， 而它的解是以估计值（常常是状态变量的估计值）的形式给出其信号模型是从状态方 程和量测方程得到的。 卡尔曼滤波中信号和噪声是用状态方程和测量方程来表示的。因此设计卡尔曼滤波 器要求已知状态方程和测量方程。它不需要知道全部过去的数据，采用递推的方法计 算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统， 因而它比维纳过滤有更广泛的应用。 卡尔曼几个重要公式： ŝ(n|n) = a ŝ (n-1|n-1) + G [x(n) – ac ŝ (n-1|n-1)] (1) n 2 P(n) = a ξ(n-1) + Q (2) () G = (3) n 2 + () ξ(n) = = (1 – cG )P(n) (4) n 这组方程的递推计算过程如图 1 所示。 图 1. 卡尔曼滤波器递推运算流程图 卡尔曼滤波过程实际上是获取维纳解的递推运算过程，这一过程从某个初始状态启 动，经过迭代运算，最终到达稳定状态，即维纳滤波状态。递推计算按图 1 所示进行。 假设已经有了初始值 ŝ(0|0)和 ξ （0 ），这样便可由式（2 ）计算P(1) ，由式（3 ）计算 G ，由式（4 ）计算ξ （1），由式（1）计算 ŝ(1|1) 。ξ （1）和ŝ(1|1)便成为下一轮迭代 1 运算的已知数据。在递推运算过程中，随着迭代次数 n 的增加，ξ （n ）将逐渐下降， 知道最终趋近于某个稳定值 ξ 。这时 0 ξ （n ） = ξ （n - 1 ）= ξ0 为求得这个稳定值，将式（3 ）和式（2 ）代入式（4 ），得到 2 2 1 ‒ + （ ） ξ02 + 2 2 ξ0 ‒ 2 2 = 0 解此方程即可求出 ξ 。 0 2. 基于 Matlab 的卡尔曼滤波器的仿真 Matlab代码如下： clear N=200; w(1)=0; x(1)=5; a=1; c=1; %过程噪声 Q1=randn(1,N)*1; %测量噪声 Q2=randn(1,N); %状态矩阵 for k=2:N;x(k)=a*x(k-1)+Q1(k-1);end for k=1:N;Y(k)=c*x(k)+Q2(k);end p(1)=10; s(1)=1; for t=2:N; Rww = cov(Q1(1:t)); Rvv = cov(Q2(1:t)); p1(t)=a.^2*