当前位置：文档之家 › 卡尔曼滤波实验及matlab实现

卡尔曼滤波实验及matlab实现

  • 格式：doc
  • 大小：791.00 KB
  • 文档页数：8

实验一 卡尔曼滤波一、 实验目的1、了解卡尔曼滤波的准则和信号模型，以及卡尔曼滤波的应用。

2、熟练掌握卡尔曼滤波的递推过程，提高对信号进行处理的能力。

3、分析讨论实际状态值和估计值的误差。

二、实验原理1、卡尔曼滤波简介卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计值和最近一个观察数据来估计信号的当前值。

它是用状态方程和递推方法进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给出其信号模型是从状态方程和量测方程得到的。

卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。

因此设计卡尔曼滤波器要求已知状态方程和测量方程。

它不需要知道全部过去的数据，采用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。

2、卡尔曼滤波的递推公式)(11∧-∧-∧-+=k k k k k k k k x A C y H x A x (1)1)(-+''=k k k k k k k R C P C C P H ττ.........(2) 11--+='k k k k k Q A P A P τ (3)k k k k P C H I P '-=)(………(4) 3、递推过程的实现如果初始状态0x 的统计特性][0x E 及]var[0x 已知，并 令 000][μ==∧x E x又]var[]))([(000000x x x x x E P =--=∧∧τ将0P 代入式（3）可求得1P '，将1P '代入式（2）可求得1H ，将此1H 代入式（1）可求得在最小均方误差条件下的∧1x ，同时将1P '代入式（4）又可求得1P；由1P又可求2P '，由2P '又可求得2H ，由2H 又可求得∧2x ，同时由2H 与2P '又可求得2P ……；以此类推，这种递推计算方法用计算机计算十分方便。

三、实验器材1、计算机一台2、MATLAB 软件一套四、实验内容一个系统模型为 )()()1(,1,0),()()()1(22211k w k x k x k k w k x k x k x +=+=++=+同时有下列条件：（1） 初始条件已知且有T x ]0,0[)0(=。

（2） )(k w 是一个标量零均值白高斯序列，且自相关函数已知为jk k w j w E δ=)]()([。

另外，我们有下列观测模型，即 )1()1()1(,1,0),1()1()1(222111+++=+=+++=+k v k x k y k k v k x k y且有下列条件：（3） )1(1+k v 和)1(2+k v 是独立的零均值白高斯序列，且有 ,2,1,0,2)]()([,)]()([2211===k k v j v E k v j v E jk jk δδ（4） 对于所有的j 和k ，)(k w 与观测噪声过程)1(1+k v 和)1(2+k v 是不相关的，即,2,1,0,,2,1,0,0)]()([,0)]()([21====k j k v j w E k v j w E我们希望得到由观测矢量)1(+k y ，即T k y k y k y )]1(),1([)1(21++=+估计状态矢量T k x k x k x )]1(),1([)1(21++=+的卡尔曼滤波器的公式表示形式，并求解以下问题：（a ） 求出卡尔曼增益矩阵，并得出最优估计)1(+k x 和观测矢量)1(),...,2(),1(+k y y y 之间的递归关系。

（b ） 通过一个标量框图（不是矢量框图）表示出状态矢量)1(+k x 中元素)1(1+k x 和)1(2+k x 估计值的计算过程。

（c ） 用模拟数据确定状态矢量)(k x 的估计值,10,...,1,0),(=∧k k k x 并画出当k ＝0，1，…，10时)(1k k x ∧和)(2k k x ∧的图。

（d ） 通常，状态矢量的真实值是得不到得。

但为了用作图来说明问题，表P8.1和P8.2给出来状态矢量元素得值。

对于k ＝0，1，…，10，在同一幅图中画出真实值和在（c ）中确定的)(1k x 的估计值。

对)(2k x 重复这样过程。

当k 从1变到10时，对每一个元素i ＝1，2，计算并画出各自的误差图，即)()(k k x k x i i ∧-。

（e ） 当k 从1变到10时，通过用卡尔曼滤波器的状态误差协方差矩阵画出)]([21k k E ε和)]([22k k E ε，而)()()(111k k x k x k k ∧-=ε，)()()(222k k x k x k k ∧-=ε。

（f ） 讨论一下（d ）中你计算的误差与（e ）中方差之间的关系。

表P8.1 题8.1到题8.3中的观测值时间下标k 观测值)(1k y )(2k y 观测值1 23456789103.296919693.387365157.028306419.7121252111.4201831515.9787058322.0693428528.3021278130.4468383138.758755952.101342940.475407973.176888982.498111402.919924246.173076165.425192743.053657415.980511414.51016361表P8.2 题8.1到题8.3中的由模拟得到的实际状态值时间下标k实际状态值)(1k x 实际状态值)(1k x 0123456789100.00000000001.654287143.503007025.9978529249.1504074012.5087391016.9219259421.3448335225.8933514431.5413533036.936056700.0000000001.654287141.848719882.475522223.171878163.358331704.413186844.422907584.548517925.648001865.394470340五、实验结果分析（a ）卡尔曼增益矩阵：k k k T T 1H P C (C P C R)-=⨯⨯⨯⨯+’’ 估计值与观测值之间的递归关系为：k k-1k-1k k k k k X A X H (Y C A X )∧∧∧=⨯+⨯-⨯⨯（b ）状态矢量估计值的计算框图：（c ）)(1k k x ∧和)(2k k x ∧的图：1ˆk x+（d ）真实值与估计值的比较图：各自的误差图：（e ）通过用卡尔曼滤波器的状态误差协方差矩阵画出的)]([21k k E ε和)]([22k k E ε：（f）分析：（e）中的方差是（d）中的误差平方后取均值，是均方误差。

误差直接由真实值减去估计值，有正有负，而均方误差没有这个缺陷，更能综合的表示滤波的效果。

附程序：%卡尔曼滤波实验程序clc;y1=[3.29691969,3.38736515,7.02830641,9.71212521,11.42018315,15.97870583 ,22.06934285,28.30212781,30.44683831,38.75875595]; %观测值y1(k)y2=[2.10134294,0.47540797,3.17688898,2.49811140,2.91992424,6.17307616,5.42519274,3.05365741,5.98051141,4.51016361]; %观测值y2(k)p0=[1,0;0,1];p=p0; %均方误差阵赋初值Ak=[1,1;0,1]; %转移矩阵Qk=[1,0;0,1]; %系统噪声矩阵Ck=[1,0;0,1]; %量测矩阵Rk=[1,0;0,2]; %测量噪声矩阵x0=[0,0]';xk=x0; %状态矩阵赋初值for k=1:10Pk=Ak*p*Ak'+Qk; %滤波方程3Hk=Pk*Ck'*inv(Ck*Pk*Ck'+Rk); %滤波方程2yk=[y1(k);y2(k)]; %观测值xk=Ak*xk+Hk*(yk-Ck*Ak*xk); %滤波方程1x1(k)=xk(1);x2(k)=xk(2); %记录估计值p=(eye(2)-Hk*Ck)*Pk; %滤波方程4pk(:,k)=[p(1,1),p(2,2)]'; %记录状态误差协方差矩阵endfigure %画图表示状态矢量的估计值subplot(2,1,1)i=1:10;plot(i,x1(i),'k')h=legend('x1(k)的估计值')set(h,'interpreter','none')subplot(2,1,2)i=1:10;plot(i,x2(i),'k')h=legend('x2(k)的估计值')set(h,'interpreter','none')X1=[0,1.65428714,3.50300702,5.997852924,9.15040740,12.50873910,16.9219 2594,21.34483352,25.89335144,31.54135330,36.93605670]; %由模拟得到的实际状态值X1(k)X2=[0,1.65428714,1.84871988,2.47552222,3.17187816,3.35833170,4.4131868 4,4.42290758,4.54851792,5.64800186,5.394470340]; %由模拟得到的实际状态值X2(k)figure %在同一幅图中画出状态矢量的估计值与真实值subplot(2,1,1)i=1:10;plot(i,x1(i),'k',i,X1(i+1),'b')h=legend('x1(k)的估计值','x1(k)的真实值')set(h,'interpreter','none')subplot(2,1,2)i=1:10;plot(i,x2(i),'k',i,X2(i+1),'b')h=legend('x2(k)的估计值','x2(k)的真实值')set(h,'interpreter','none')for i=1:10 %计算x(k)的误差e1(i)=X1(i+1)-x1(i);e2(i)=X2(i+1)-x2(i);endfigure %画出误差图subplot(2,1,1)i=1:10;plot(i,e1(i),'r')h=legend('x1(k)的误差')set(h,'interpreter','none')subplot(2,1,2)i=1:10;plot(i,e2(i),'r')h=legend('x2(k)的误差')set(h,'interpreter','none')figure %通过用卡尔曼滤波器的状态误差协方差矩阵画出E[ε1(k/k)^2]和E[ε2(k/k)^2]i=1:10;subplot(2,1,1)plot(i,pk(1,i),'r')h= legend('由状态误差协方差矩阵得到的E[ε1(k/k)^2]')set(h,'Interpreter','none')subplot(2,1,2)plot(i,pk(2,i),'r')h= legend('由状态误差协方差矩阵得到的E[ε2(k/k)^2]')set(h,'Interpreter','none')。

合集下载

卡尔曼滤波实验及matlab实现

卡尔曼滤波实验及matlab实现

实验一 卡尔曼滤波一、 实验目的1、了解卡尔曼滤波的准则和信号模型，以及卡尔曼滤波的应用。

2、熟练掌握卡尔曼滤波的递推过程，提高对信号进行处理的能力。

3、分析讨论实际状态值和估计值的误差。

二、实验原理1、卡尔曼滤波简介卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计值和最近一个观察数据来估计信号的当前值。

它是用状态方程和递推方法进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给出其信号模型是从状态方程和量测方程得到的。

卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。

因此设计卡尔曼滤波器要求已知状态方程和测量方程。

它不需要知道全部过去的数据，采用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。

2、卡尔曼滤波的递推公式)(11∧-∧-∧-+=k k k k k k k k x A C y H x A x (1)1)(-+''=k k k k k k k R C P C C P H ττ.........(2) 11--+='k k k k k Q A P A P τ (3)k k k k P C H I P '-=)(………(4) 3、递推过程的实现如果初始状态0x 的统计特性][0x E 及]var[0x 已知，并 令 000][μ==∧x E x又]var[]))([(000000x x x x x E P =--=∧∧τ将0P 代入式（3）可求得1P '，将1P '代入式（2）可求得1H ，将此1H 代入式（1）可求得在最小均方误差条件下的∧1x ，同时将1P '代入式（4）又可求得1P；由1P又可求2P '，由2P '又可求得2H ，由2H 又可求得∧2x ，同时由2H 与2P '又可求得2P ……；以此类推，这种递推计算方法用计算机计算十分方便。

卡尔曼滤波器Matlab仿真

卡尔曼滤波器Matlab仿真

用于雷达跟踪的卡尔曼滤波器
——Matlab仿真分析
L/O/G/O
1. 案例条件设定
1.一个二座标雷达对一平面上运动的目 标进行观测。雷达扫描周期T=2s，对x 和y独立地进行观测，观测噪声的标准 差均为100m； 2.目标的起始点为(2000,10000)，单位 为米(m)。目标在t=0至400s沿y轴作恒 速直线运动，运动速度为-15m/s； 3.在400至600s向轴方向做90度的慢转 弯，加速度为0.075m/s； 4.完成慢转弯后加速度降为零，从610s 开始做的快转弯，加速度为0.3m/s，在 660s结束转弯，加速度降至零。
而预测的误差协方差可由下式表出:
对于最佳滤波，迭代表达式为：
2. Kalman滤波算法 Kalman滤波算法
滤波误差的协方差为：
在应用上面的公式进行Kalman滤波时，需要指定初值。由于实际中通常 无法得到目标的初始状态，我们可以利用前几个观测值建立状态的初始 估计，比如采用前两个观测值:
估计误差为:
2. Kalman滤波算法 Kalman滤波算法
对x轴方向进行讨论(y轴同理)： 轴方向进行讨论(y轴同理) (y轴同理 目标沿x轴方向的运动可以用以下方程描述：
用矩阵的形式表述为：
在上式中:
2. Kalman滤波算法 Kalman滤波算法
考虑雷达的观测，得出观测方程为：
其中C(k)=[1 0],V(k)为零均值的噪声序列，方差已知。 对目标进行预测，由相关理论可得到下面的迭代式：
理论航迹与滤波估计以看出Kalman滤波算法对于动态目标的跟踪 有着比较好的效果，能够较好地抑止环境中的噪声影响。
谢谢观赏
L/O/G/O
@Made by Jimson @现代数字信号处理
rssi卡尔曼滤波matlab代码

rssi卡尔曼滤波matlab代码

rssi卡尔曼滤波matlab代码RSSI (Received Signal Strength Indicator) 是一种常见的无线通信信号强度测量方法。

在无线传感器网络中，RSSI常用于距离和位置估计。

卡尔曼滤波器是一种高效递归滤波器，可用于在有噪声的情况下，根据一系列测量值来估计状态变量。

在无线传感器网络中，卡尔曼滤波器可用于RSSI测量值的滤波和校正。

以下是一个简单的RSSI卡尔曼滤波器的MATLAB代码示例：```matlab% 假设你已经有了原始的RSSI测量值 rssi_measurements% 初始位置和速度x0 = [0, 0]; % 初始位置P0 = 1; % 初始位置的不确定性Q = 0.01; % 过程噪声协方差R = 1; % 测量噪声协方差% 卡尔曼滤波器参数dt = 0.1; % 时间间隔x = x0; % 当前位置P = P0; % 当前位置的不确定性K = zeros(2,1); % 卡尔曼增益for i = 1:length(rssi_measurements)% 预测步骤：状态转移方程x_pred = x + dt * x; % 预测位置P_pred = P + Q; % 预测位置的不确定性% 更新步骤：测量更新方程Z = rssi_measurements(i) + x_pred(2)^2 / (x_pred(1)^2 + x_pred(2)^2) - x_pred(1)^2 / (x_pred(1)^2 + x_pred(2)^2); % 计算测量值K = P_pred / (P_pred + R); % 计算卡尔曼增益x = x_pred + K * (Z - x_pred(1)); % 更新位置P = (1 - K) * P_pred; % 更新位置的不确定性end```请注意，这个代码只是一个简单的示例，并没有考虑所有可能的情况和参数。

在实际应用中，你可能需要根据具体的需求和环境条件来调整和优化这个代码。

基于MATLAB的卡尔曼滤波算法实现

基于MATLAB的卡尔曼滤波算法实现

基于MATLAB的卡尔曼滤波算法实现以下为纯⼩⽩对卡尔曼滤波算法的MATLAB实现进⾏简单总结，以便以后复习。

⼀、背景介绍假设⼀⼩车作匀加速运动，初速度为0，加速度为5⽶每⼆次⽅秒，⼩车上装有速度传感器，采样频率为10Hz，传感器测量噪声为⾼斯⽩噪声，需要充分利⽤这些信息来估计车辆的速度状态，并验证卡尔曼滤波算法的实验原理与过程。

⼆、卡尔曼滤波原理早在近百年前，就有⼈开始采⽤状态变量模型研究随机过程，随后为了解决对⾮平稳、多输⼊输出随机序列的估计问题，卡尔曼提出了递推最优估计理论。

它采⽤状态空间法描述系统，由状态⽅程和测量⽅程组成，即知道前⼀个状态的估计值和最近⼀个观测数据，采⽤递推算法估计当前的状态值。

其具有以下特点：（1）算法是⼀个递推过程，且状态空间法采⽤在时域内设计滤波器的⽅法，因⽽适⽤于多维随机过程的估计；离散型卡尔曼滤波算法适⽤于计算机处理。

（2）采⽤递推算法计算，不⽤知道所有过去的值，⽤状态⽅程描述状态变量的动态变化规律，卡尔曼滤波同样适⽤于⾮平稳过程。

如图所⽰，为卡尔曼滤波算法的实现流程图，其基本思路是若有⼀组强⽽合理的假设，给出系统的历史测量值，可以建⽴最⼤化这些早前测量值的后验概率的系统状态模型。

其基本假设是被建系统是线性的，影响测量的噪声属于符合⾼斯分布的⽩噪声。

三、卡尔曼滤波的状态⽅程和测量⽅程整个实现过程包含预测更新和测量更新两⼤部分。

（1）预测更新：预测状态量：预测误差协⽅差矩阵：（2）测量更新：最优估计状态量：计算误差增益：误差协⽅差矩阵：设k时刻⼩车速度为，则系统状态⽅程为：测量⽅程为：结合卡尔曼滤波算法的预测和测量更新流程，可有：四、结果分析根据以上理论分析，完成相应matlab程序（代码见后），并画出⼩车速度的观测值、真实值、滤波值的对⽐曲线图如下：五、代码(1) kalman.m1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%2 %⽇期：2020.3.103 %程序功能：使⽤卡尔曼滤波器估计⼩车匀加速运动时的速度状态4 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 clear6 clc7 clear all8 A = [1];9 B = [1];10 U = 0.5; %A,B,U初始值来⾃于状态⽅程11 H = [1]; %H来⾃于测量⽅程12 step = 500;13 v = normrnd(0,10,1,step); %均值为0，⽅差为10噪声14 X_0 = zeros(1,step); %先验估计初始值15 X0 = zeros(1,step); %后验估计初始值16 Z = zeros(1,step); %观测值初始值17true = 0.5*(1:step); %⼩车真实速度18 temp = true + v(1,:);19 Z(1,:) = temp; %加⼊噪声的速度，模拟观测值20 Q = [10]; %状态⽅程噪声协⽅差矩阵21 R = [500]; %测量⽅程协⽅差矩阵22 P0 = [1]; %初始后验估计协⽅差23 P_0 = [1];24for i = 2:step25 [X_0(i),P_0] = kalmanPredictor(A,B,U,P0,Q,X0(i-1));26 [X0(i),P0] = kalmanUpdater(H,R,P_0,Z(i),X_0(i));27 end28 figure;29 plot(Z,'g.');30 hold on;31 plot(true,'r.');32 hold on;33 plot(X0,'b');34 grid on;35 legend('观测值','真实值','kalman滤波');(2)kalmanPredictor.mfunction [Xp,Pp] = kalmanPredictor(A,B,U,P0,Q,X0)Xp = A*X0 + B*U;Pp = A*P0*A' + Q;end(3)kalmanUpdater.mfunction [Xup,Pup] = kalmanUpdater(H,R,Pp,Z,Xp)I = eye(size(Xp,1));K = (Pp*H')/(H*Pp*H' + R);Xup = Xp + K*(Z-H*Xp);Pup = (I-K*H)*Pp;end。