您的位置：360文档中心› 无迹卡尔曼滤波UKF无线传感器网络定位跟踪matlab源码实现

无迹卡尔曼滤波UKF无线传感器网络定位跟踪matlab源码实现

合集下载

无迹卡尔曼滤波matlab

无迹卡尔曼滤波matlab

无迹卡尔曼滤波matlab无迹卡尔曼滤波（Unscented Kalman Filter，简称UKF）是一种基于卡尔曼滤波的非线性滤波方法。

与传统的卡尔曼滤波方法相比，UKF能够更好地处理非线性系统，并且不需要对系统进行线性化处理，因此在实际应用中具有广泛的应用价值。

UKF的基本思想是通过引入一组状态变量来描述系统的状态，然后使用卡尔曼滤波的方法对系统状态进行估计。

不同于传统的线性卡尔曼滤波方法，UKF使用一组称为Sigma点的采样点来代替状态变量的传统线性化处理，从而使得滤波结果更加准确。

在实际应用中，UKF可以用于各种非线性系统的估计和控制，例如机器人导航、飞行器控制等。

下面以一个简单的例子来介绍UKF的应用。

假设我们需要对一个匀加速直线运动的物体进行状态估计，其状态包括位置、速度和加速度三个变量。

我们可以将系统状态表示为一个三维向量x=[p,v,a]，其中p、v和a分别表示位置、速度和加速度。

我们需要确定系统的运动模型。

对于匀加速直线运动，其运动模型可以表示为：x(k+1) = Fx(k) + w(k)其中，F是状态转移矩阵，w(k)是过程噪声。

假设噪声服从高斯分布，我们可以将其表示为：w(k) ~ N(0,Q)其中，Q是噪声协方差矩阵。

接下来，我们需要确定系统的观测模型。

假设我们可以通过某些传感器得到物体的位置观测值z(k)，则观测模型可以表示为：z(k) = Hx(k) + v(k)其中，H是观测矩阵，v(k)是观测噪声。

同样地，假设观测噪声服从高斯分布，我们可以将其表示为：v(k) ~ N(0,R)其中，R是观测噪声协方差矩阵。

有了系统的运动模型和观测模型，我们就可以使用UKF对系统状态进行估计了。

UKF的主要步骤如下：1. 选择一个合适的Sigma点集合，通过这些点来对系统状态进行采样；2. 将Sigma点通过运动模型进行状态转移，从而得到预测状态；3. 通过预测状态和观测模型，计算预测观测值；4. 通过卡尔曼增益和观测值，对预测状态进行修正，得到最终估计状态。

UKF在Matlab下的实现

UKF在Matlab下的实现

UKF在Matlab下的实现西安交通大学刘继冬1.实验综述实验综述：：无迹卡尔曼滤波UKF 是重要的非线性滤波方法。

它采用UT 变换的方法，不再近似系统的非线性方程，它仍然用高斯随机变量表示状态分布，不过是用特定选择的样本点加以描述，每个点叫一个高斯点，它从系统状态的概率密度函数中取出；然后，按系统的真实模型演化，得到非线性演化后的σ点，使得样本均值和样本方差是真实均值和真实方差的好的近似。

在这次实验中，实现了基于UKF的滤波方法，并且建立了两种仿真环境进行实验。

2.2.仿真环境设置仿真环境设置仿真环境设置：：（1）仿真仿真环境环境环境11：一维的线性直线匀加速运动，理想的初始速度为0，位置为0，加速度为1。

速度，位置和加速度均有一定的误差，且误差可能较大，误差系数约为0.3。

同时，测量系统只能检测到当前的速度，其余是0，而且干扰较大。

系统的状态方程为：11t 0X X Q 01t 001K K K +=+测量方程为：[]1Z 010X R K K K +=+状态变量X=（x1，x2，x3）T ，其中，x1为位置，x2为速度，x3为加速度。

（2）仿真仿真环境环境环境22：二维平面的匀加速运动，系统的初始状态为：处于（0.3，0.2）位置，初始速度为X轴为1，Y轴为2，加速度为X方向为2，Y方向为-1。

测量方程为非线性。

若简化为线性系统，为了满足能观性，选取可以测量的量不仅为当前的速度，也有当前的位置和加速度。

状态方程为：110t 000010t 00X X Q 00010t 000010000001K K K +=+测量方程为：1Z h(X )R K K K +=+h=[(x(1)+1)^0.5;0.8*x(2)+0.3*x(1);x(3);x(4);x(5);x(6)] 状态变量X=（x1，x2，x3，x4，x5，x6）T ，其中，x1为X轴位置，x2为Y轴位置，x3、x4分别X，Y轴的速度，x5、x6为两方向的加速度。

matlab 无线传感器 实验代码

matlab 无线传感器 实验代码

一、介绍matlab无线传感器实验代码的作用和重要性1. 无线传感器在现代科技中的重要性2. Matlab作为无线传感器实验的常用工具3. 无线传感器实验代码的编写和应用范围二、matlab无线传感器实验代码的基本框架及功能1. 无线传感器实验代码的基本框架概述2. Matlab在无线传感器实验中的应用功能3. 无线传感器实验代码的具体功能介绍三、matlab无线传感器实验代码的编写流程和技巧1. 无线传感器实验代码的编写步骤2. Matlab编程技巧在无线传感器实验中的应用3. 案例分析：matlab实现无线传感器数据采集和处理的代码编写流程四、matlab无线传感器实验代码的应用案例1. 传感器网络数据处理和分析2. 无线传感器节点控制和管理3. 无线传感器实验代码在工业生产中的应用实例分析五、matlab无线传感器实验代码的发展趋势1. 无线传感器技术的发展现状2. Matlab在无线传感器实验中的创新应用3. 未来matlab无线传感器实验代码的发展方向和趋势结尾：matlab无线传感器实验代码的未来展望和重要性总结1. 无线传感器技术和matlab在科学研究中的重要性2. 对matlab无线传感器实验代码的期望和推广应用3. 未来matlab无线传感器实验代码的发展方向和应用前景六、matlab无线传感器实验代码的应用案例无线传感器网络已经被广泛应用于许多领域，如环境监测、智能交通、智能家居、农业监测、医疗保健等。

在这些领域，无线传感器实验代码在数据采集、数据处理、节点控制等方面发挥着重要作用。

1. 传感器网络数据处理和分析通过matlab无线传感器实验代码，我们可以对传感器网络中的数据进行高效的处理和分析。

传感器网络中的数据通常具有时间序列、空间分布等特点，在处理这些数据时，我们需要进行信号处理、滤波、数据融合等操作，而matlab提供了丰富的工具箱和函数库，能够较为方便地实现这些操作。

卡尔曼滤波在船舶gps导航定位系统中的应用的matlab代码

卡尔曼滤波在船舶gps导航定位系统中的应用的matlab代码

卡尔曼滤波是一种在信号处理和预测领域中广泛应用的技术，尤其在船舶导航定位系统中。

下面是一个简单的卡尔曼滤波器的MATLAB代码示例，它可能被用于船舶GPS导航定位系统。

请注意，这只是一个基础的例子，实际的系统可能需要更复杂的卡尔曼滤波器，例如考虑多路径效应、大气干扰、风速变化等。

```matlab% 初始化time = 0:0.01:10; % 时间向量x = randn(size(time)); % 初始状态P = randn(size(time)); % 初始协方差矩阵Q = randn(size(time)); % 过程噪声协方差R = randn(size(time)); % 过程噪声协方差矩阵K = zeros(size(time)); % 卡尔曼增益矩阵x_est = zeros(size(time)); % 估计状态P_est = zeros(size(time)); % 估计协方差矩阵% 卡尔曼滤波循环for i = 1:length(time)% 当前时间观测z = x + sqrt(Q)*randn(); % 假设GPS信号是一个白噪声过程K = P_est ./ (P_est + R); % 卡尔曼增益x_est(i) = x(i) + K * (z - P_est(:,i)); % 估计状态更新P_est(i,:) = (I - K * P_est(:,i)') * P; % 估计协方差矩阵更新K = zeros(size(time)); % 重置卡尔曼增益矩阵end% 绘制结果figure; plot(time, x); hold on;figure; plot(time, x_est); hold off;legend('真实值', '估计值');```这个代码主要实现了一个简单的卡尔曼滤波器，用于对船舶GPS导航定位系统的数据进行滤波处理。

基于扩展卡尔曼滤波的目标跟踪定位算法及matlab程序实现

基于扩展卡尔曼滤波的目标跟踪定位算法及matlab程序实现

基于扩展卡尔曼滤波的目标跟踪定位算法及matlab程序实现扩展卡尔曼滤波（Extended Kalman Filter，EKF）是一种用于非线性系统状态估计的算法。

在目标跟踪定位中，它可以用于估计目标的运动轨迹。

下面是一个简单的基于扩展卡尔曼滤波的目标跟踪定位算法的描述，以及一个简化的MATLAB程序实现。

算法描述1. 初始化：设置初始状态估计值（例如位置和速度）以及初始的估计误差协方差矩阵。

2. 预测：根据上一时刻的状态估计值和模型预测下一时刻的状态。

3. 更新：结合观测数据和预测值，使用扩展卡尔曼滤波算法更新状态估计值和估计误差协方差矩阵。

4. 迭代：重复步骤2和3，直到达到终止条件。

MATLAB程序实现这是一个简化的示例，仅用于说明扩展卡尔曼滤波在目标跟踪定位中的应用。

实际应用中，您需要根据具体问题和数据调整模型和参数。

```matlab% 参数设置dt = ; % 时间间隔Q = ; % 过程噪声协方差R = 1; % 观测噪声协方差x_est = [0; 0]; % 初始位置估计P_est = eye(2); % 初始估计误差协方差矩阵% 模拟数据：观测位置和真实轨迹N = 100; % 模拟数据点数x_true = [0; 0]; % 真实轨迹初始位置for k = 1:N% 真实轨迹模型（这里使用简化的匀速模型）x_true(1) = x_true(1) + x_true(2)dt;x_true(2) = x_true(2);% 观测模型（这里假设有噪声）z = x_true + sqrt(R)randn; % 观测位置% 扩展卡尔曼滤波更新步骤[x_est, P_est] = ekf_update(x_est, P_est, z, dt, Q, R);end% 扩展卡尔曼滤波更新函数（这里简化为2D一维情况）function [x_est, P_est] = ekf_update(x_est, P_est, z, dt, Q, R)% 预测步骤：无观测时使用上一时刻的状态和模型预测下一时刻状态F = [1 dt; 0 1]; % 状态转移矩阵（这里使用简化的匀速模型）x_pred = Fx_est + [0; 0]; % 预测位置P_pred = FP_estF' + Q; % 预测误差协方差矩阵% 更新步骤：结合观测数据和预测值进行状态更新和误差协方差矩阵更新K = P_predinv(HP_pred + R); % 卡尔曼增益矩阵x_est = x_pred + K(z - Hx_pred); % 更新位置估计值P_est = (eye(2) - KH)P_pred; % 更新误差协方差矩阵end```这个示例代码使用扩展卡尔曼滤波对一个简化的匀速运动模型进行估计。

卡尔曼滤波器及matlab实现

卡尔曼滤波器及matlab实现

卡尔曼滤波器及Matlab实现简介卡尔曼滤波器是一种常用于估计系统状态的滤波器，特别适用于具有线性动态模型和高斯噪声的系统。

它通过结合系统的测量值和模型预测的状态来估计系统的状态，并利用测量噪声和模型噪声的特性进行优化。

本文将介绍卡尔曼滤波器的基本原理，并使用Matlab实现一个简单的卡尔曼滤波器。

卡尔曼滤波器的基本原理卡尔曼滤波器的基本原理可以描述为以下步骤：1.初始化卡尔曼滤波器的状态估计值和协方差矩阵。

通常情况下，可以将初始状态设定为系统的初始状态，协方差矩阵设定为一个较大的值。

2.预测步骤：根据系统的动态模型预测下一时刻的状态和协方差矩阵。

3.更新步骤：使用测量值来更新预测的状态和协方差矩阵，得到最优的状态估计值和协方差矩阵。

具体的数学表达式如下：预测步骤：预测的状态估计值：x_k = A*x_(k-1) + B*u_k预测的协方差矩阵：P_k = A*P_(k-1)*A' + Q其中，A是状态转移矩阵，B是输入控制矩阵，u_k是输入控制向量，Q是模型噪声协方差。

更新步骤：测量残差：y_k = z_k - H*x_k残差协方差矩阵：S_k = H*P_k*H' + R卡尔曼增益：K_k = P_k*H'*inv(S_k)更新后的状态估计值：x_k = x_k + K_k*y_k更新后的协方差矩阵：P_k = (I - K_k*H)*P_k其中，H是观测矩阵，z_k是测量值，R是测量噪声协方差。

Matlab实现接下来，我们使用Matlab来实现一个简单的卡尔曼滤波器。

我们假设一个一维运动系统，系统状态为位置，系统模型如下：x_k = x_(k-1) + v_(k-1) * dtv_k = v_(k-1) + a_(k-1) * dt式中，x_k是当前时刻的位置，v_k是当前时刻的速度，a_k是当前时刻的加速度，dt是时间步长。

假设我们只能通过传感器得到位置信息，并且测量噪声服从均值为0、方差为0.1的高斯分布。

无损卡尔曼滤波UKF Matlab程序

无损卡尔曼滤波UKF Matlab程序

ukf(无迹卡尔曼滤波)算法的matlab程序. function [x,P]=ukf(fstate,x,P,hmeas,z,Q,R)% UKF Unscented Kalman Filter for nonlinear dynamic systems% [x, P] = ukf(f,x,P,h,z,Q,R) returns state estimate, x and state covariance, P% for nonlinear dynamic system (for simplicity, noises are assumed as additive): % x_k+1 = f(x_k) + w_k% z_k = h(x_k) + v_k% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q% v ~ N(0,R) meaning v is gaussian noise with covariance R% Inputs: f: function handle for f(x)% x: "a priori" state estimate% P: "a priori" estimated state covariance% h: fanction handle for h(x)% z: current measurement% Q: process noise covariance% R: measurement noise covariance% Output: x: "a posteriori" state estimate% P: "a posteriori" state covariance%% Example:%{n=3; %number of stateq=0.1; %std of processr=0.1; %std of measurementQ=q^2*eye(n); % covariance of processR=r^2; % covariance of measurementf=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equationsh=@(x)x(1); % measurement equations=[0;0;1]; % initial statex=s+q*randn(3,1); %initial state % initial state with noiseP = eye(n); % initial state covraianceN=20; % total dynamic stepsxV = zeros(n,N); %estmate % allocate memorysV = zeros(n,N); %actualzV = zeros(1,N);for k=1:Nz = h(s) + r*randn; % measurmentssV(:,k)= s; % save actual statezV(k) = z; % save measurment[x, P] = ukf(f,x,P,h,z,Q,R); % ekfxV(:,k) = x; % save estimates = f(s) + q*randn(3,1); % update processendfor k=1:3 % plot resultssubplot(3,1,k)plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--')end%}%% By Yi Cao at Cranfield University, 04/01/2008%L=numel(x); %numer of statesm=numel(z); %numer of measurementsalpha=1e-3; %default, tunableki=0; %default, tunablebeta=2; %default, tunablelambda=alpha^2*(L+ki)-L; %scaling factorc=L+lambda; %scaling factorWm=[lambda/c 0.5/c+zeros(1,2*L)]; %weights for meansWc=Wm;Wc(1)=Wc(1)+(1-alpha^2+beta); %weights for covariancec=sqrt(c);X=sigmas(x,P,c); %sigma points around x[x1,X1,P1,X2]=ut(fstate,X,Wm,Wc,L,Q); %unscented transformation of process % X1=sigmas(x1,P1,c); %sigma points around x1% X2=X1-x1(:,ones(1,size(X1,2))); %deviation of X1[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R); %unscented transformation of measurmentsP12=X2*diag(Wc)*Z2'; %transformed cross-covarianceK=P12*inv(P2);x=x1+K*(z-z1); %state updateP=P1-K*P12'; %covariance updatefunction [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)%Unscented Transformation%Input:% f: nonlinear map% X: sigma points% Wm: weights for mean% Wc: weights for covraiance% n: numer of outputs of f% R: additive covariance%Output:% y: transformed mean% Y: transformed smapling points% P: transformed covariance% Y1: transformed deviationsL=size(X,2);y=zeros(n,1);Y=zeros(n,L);for k=1:LY(:,k)=f(X(:,k));y=y+Wm(k)*Y(:,k);endY1=Y-y(:,ones(1,L));P=Y1*diag(Wc)*Y1'+R;function X=sigmas(x,P,c)%Sigma points around reference point%Inputs:% x: reference point% P: covariance% c: coefficient%Output:% X: Sigma pointsA = c*chol(P)';Y = x(:,ones(1,numel(x))); X = [x Y+A Y-A];。

(整理)卡尔曼滤波简介及其算法MATLAB实现代码.

(整理)卡尔曼滤波简介及其算法MATLAB实现代码.
P(k|k-1)=A P(k-1|k-1) A’+Q………(2)
式(2)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的covariance，A’表示A的转置矩阵，Q是系统过程的covariance。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。
首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述：
X(k)=A X(k-1)+B U(k)+W(k)
再加上系统的测量值：
Z(k)=H X(k)+V(k)
上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance分别是Q，R（这里我们假设他们不随系统状态变化而变化）。
现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)………(4)
现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。
  1. 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
  2. 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
  3. 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。

%本例对基于量测非线性模型（正切），进行了仿真；通过对比分析EKF，UKF和PF粒子滤波的性能。

仿真结果可以看出粒子滤波器比UKF优越，UKF比EKF性能优越。

可作为学习滤波器的参考资料。

%存在问题：目前修正效果还不够完美，滤波值在预测值的基础上有所改善，使其接近真实值。

clear all;close all;clc;%Clear command window.st = 100; % simulation length(time)MC=50; %仿真次数dl=zeros(MC,st+1);de=zeros(MC,st+1);dp=zeros(MC,st+1);%仿真10次for time=1:MCdl(time,1)=0;de(time,1)=0;dp(time,1)=0;Q = 0.5; % process noise covarianceR = [3^2 0;0 0.1745^2 ];% measurement noise covariancex0 = [0,5,0,7]'; % initial statex = x0;xA = [x(1)];%Array:Save the true X -positionyA = [x(3)];%Array:Save Y-Positionxobs = [x(1)]; %观测到的坐标yobs = [x(3)];ZA = [];%初始化系统方程系数CV线性模型F=[ 1.0 1.0 0.0 0.0;0.0 1.0 0.0 0.0;0.0 0.0 1.0 1.0;0.0 0.0 0.0 1.0];G=[0.5 0.0;1.0 0.0;0.0 0.5;0.0 1.0];%事先得到整体过程的实际状态值和观测值for k = 1 : st%two equationx = F * x + G * normrnd(0,Q,2,1); %状态方程if x(1)>0 && x(3)>=0z(:,:,k) =[sqrt(x(1)^2+x(3)^2) atan(x(3)/x(1))]' + [normrnd(0,3^2,1,1) normrnd(0,0.1745^2,1,1)]'; %观测方程xobs = [xobs z(1,1,k)*cos(z(2,1,k))];yobs = [yobs z(1,1,k)*sin(z(2,1,k))];endif x(1)<0 && x(3)>=0z(:,:,k) =[sqrt(x(1)^2+x(3)^2) (atan(x(3)/x(1))+pi)]' + [normrnd(0,3^2,1,1) normrnd(0,0.1745^2,1,1)]';%观测方程xobs = [xobs z(1,1,k)*cos(z(2,1,k))];yobs = [yobs z(1,1,k)*sin(z(2,1,k))];endif x(1)<0 && x(3)<=0z(:,:,k) =[sqrt(x(1)^2+x(3)^2) (atan(x(3)/x(1))-pi)]' + [normrnd(0,3^2,1,1) normrnd(0,0.1745^2,1,1)]';%观测方程xobs = [xobs z(1,1,k)*cos(z(2,1,k))];yobs = [yobs z(1,1,k)*sin(z(2,1,k))];endif x(1)>0 && x(3)<=0z(:,:,k) =[sqrt(x(1)^2+x(3)^2) atan(x(3)/x(1))]' + [normrnd(0,3^2,1,1) normrnd(0,0.1745^2,1,1)]'; %观测方程xobs = [xobs z(1,1,k)*cos(z(2,1,k))];yobs = [yobs z(1,1,k)*sin(z(2,1,k))];endxA = [xA x(1)];yA = [yA x(3)];dl(time,k+1)= sqrt((xobs(k+1)-x(1))^2+(yobs(k+1)-x(3))^2);end%k = 0:st;%plot(xA,yA,'b*',0,0,'ro');%xlabel('x'); ylabel('y');%legend('Target Position','Observation Position');% UKFPu = [2,0,0,0;0,0.5,0,0;0,0,3,0;0,0,0,0.5].^2; %协方差矩阵初始化xgeu = x0;xgeAu = [xgeu(1)]; %后面代表的是滤波后的估计位置ygeAu = [xgeu(3)];xPru = [xgeu(1)]; %代表每一步目标的预测位置yPru = [xgeu(3)];alpha = 0.01; %0.5beta = 2;nnn = 4;kappa = -1;lamda = alpha^2*(nnn+kappa) - nnn;%一般的方法：W0=v/(v+n),Wi=0.5/(v+n),i=1,...,2n;一般(v+n)==3 %这儿n=4,v=-1;W0=-1/3,Wi=1/6wm = lamda/(nnn+lamda);wc = wm+(1+beta-alpha^2);for i = 1:2*nnnt = 1/(2*(lamda+nnn));wm =[ wm t];wc =[ wc t];endfor k = 1 : st% UKF滤波器n=4,a=0.01,b=2,r=4*0.01^2-4 ,w0m= wxx = [xgeu];Psqrtm=(chol((nnn+lamda)*Pu))';xPts=[zeros(size(Pu,1),1) -Psqrtm Psqrtm];wxx = xPts + repmat(wxx,1,2*nnn+1);for j = 1:2*nnn+1wxx(:,j) = F * wxx(:,j);xgepredu = wm(1) * wxx(:,1);for j = 2:2*nnn+1xgepredu = xgepredu + wm(j) * wxx(:,j);endxPru = [xPru xgepredu(1)]; %目标的预测位置yPru = [yPru xgepredu(3)];Pupred = Q*eye(4);for j = 1:2*nnn+1Pupred = Pupred + wc(j)*(wxx(:,j) - xgepredu)*(wxx(:,j) - xgepredu)'; end%xgeu = F * xgeu;%Pu = F * Pu * F' + G*Q*G';%wxx = [xgeu];%for j = 1:nnn% t = xgeu + sqrt(((nnn + rmda)*Pu(:,j)));% wxx = [wxx t];%end%for j =1:nnn% t = xgeu - sqrt(((nnn + rmda)*Pu(:,j)));% wxx = [wxx t];%end%Zkkfor j = 1:2*nnn+1if wxx(1,j)>0 && wxx(3,j)>=0wzz(:,:,j) = [sqrt(wxx(3,j)^2+wxx(1,j)^2) atan(wxx(3,j)/wxx(1,j))]';endif wxx(1,j)<0 && wxx(3,j)>=0wzz(:,:,j) = [sqrt(wxx(3,j)^2+wxx(1,j)^2) (atan(wxx(3,j)/wxx(1,j))+pi)]';endif wxx(1,j)<0 && wxx(3,j)<=0wzz(:,:,j) = [sqrt(wxx(3,j)^2+wxx(1,j)^2) (atan(wxx(3,j)/wxx(1,j))-pi)]';endif wxx(1,j)>0 && wxx(3,j)<=0wzz(:,:,j) = [sqrt(wxx(3,j)^2+wxx(1,j)^2) atan(wxx(3,j)/wxx(1,j))]';endendzku = wm(1) * wzz(:,:,1);for j = 2:2*nnn+1zku = zku + wm(j) * wzz(:,:,j);endPzu = R;for j = 1:2*nnn+1Pzu = Pzu + wc(j)*(wzz(:,:,j) - zku)*(wzz(:,:,j) - zku)';endPxzu = wc(1)*(wxx(:,1) - xgepredu)*(wzz(:,:,1) - zku)'; %这里的xgeu是一步预测值for j = 2:2*nnn+1Pxzu = Pxzu + wc(j)*(wxx(:,j) - xgepredu)*(wzz(:,:,j) - zku)';end%更新Ku = Pxzu*inv(Pzu);xgeu = xgepredu + Ku*( z(:,:,k) - zku);Pu = Pupred - Ku * (Pzu) * Ku'; %(Pzu)^(-1) ErrorxgeAu = [xgeAu xgeu(1)];%ygeAu = [ygeAu xgeu(3)];%de(time,k+1)=sqrt((xgeAu(k+1)-xA(k+1))^2+(ygeAu(k+1)-yA(k+1))^2);%dp(time,k+1)=sqrt((xPru(k+1)-xA(k+1))^2+(yPru(k+1)-yA(k+1))^2);end%显示跟踪曲线%k = 0:st;%figure;%plot(xA,yA,'r*-',xgeAu,ygeAu,'g+:',xobs,yobs,'b-x');%xA,yA,'b*',%xlabel('x'); ylabel('y');%legend('True','UKF','Observations');end%统计观测误差曲线与滤波误差曲线dlave=zeros(1,st+1);deave=zeros(1,st+1);dpave=zeros(1,st+1);for i=1:(st+1)dlave(i)=mean(dl(:,i));deave(i)=mean(de(:,i));dpave(i)=mean(dp(:,i));endfigure(1);i=0;plot(i,mean(dlave),'b-o',i,mean(deave),'g-*'); % ,i,mean(dpave),'r-s' figure(2);i=1:(st+1);plot(i,dlave,'b-o',i,deave,'g-*'); % ,i,dpave,'r-s'。

相关文档
最新文档