kalman滤波在不同信噪比时的误码率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 @现代数字信号处理
MATLAB技术卡尔曼滤波教程
MATLAB技术卡尔曼滤波教程MATLAB技术：卡尔曼滤波教程随着现代科技的发展，数据处理和信号滤波成为许多领域研究的重要环节。
其中，卡尔曼滤波作为一种常用的最优估计方法，被广泛应用于控制与导航、机器人、经济学以及信号处理等众多领域。
本文将为读者简要介绍MATLAB中的卡尔曼滤波原理与实现方法。
一、卡尔曼滤波简介卡尔曼滤波由Rudolph E. Kalman在1960年代初提出，其基本思想是通过综合当前观测数据和已知系统动态方程，估计出系统状态的最优解。
卡尔曼滤波通过联合考虑信号的测量和系统模型的不确定性，提供了一种在噪声干扰存在下的最优估计方法。
卡尔曼滤波的核心思想是建立一种递推的状态估计过程，即通过使用上一步的估计结果以及当前时刻的观测数据，预测下一步的状态。
卡尔曼滤波算法分为两个主要步骤：预测（时间更新）和更新（测量更新）。
预测步骤利用系统的动态模型和上一步的状态估计，计算出当前时刻状态的预测值以及预测误差协方差矩阵。
更新步骤则通过结合当前时刻的实际观测数据和预测值，计算出当前时刻的状态估计值和更新后的误差协方差矩阵。
二、MATLAB中的卡尔曼滤波工具箱为了解决卡尔曼滤波的数学推导与实现问题，MATLAB提供了专门的卡尔曼滤波工具箱。
该工具箱提供了丰富的函数和工具，使得用户可以方便地进行卡尔曼滤波算法的实现与仿真。
首先，用户需要定义系统的动态模型和测量模型，并设置初始状态以及误差协方差矩阵。
MATLAB中提供了`kalman`函数用于实现卡尔曼滤波的状态更新与估计。
其次，用户可以利用`kalman`函数进行滤波的仿真实验。
通过输入实际观测数据以及系统模型，用户可以获得滤波后的状态估计值和误差协方差矩阵。
此外，用户还可以根据系统模型的不同，选择不同的卡尔曼滤波算法（如扩展卡尔曼滤波、无迹卡尔曼滤波等）。
三、实例演示：基于MATLAB的卡尔曼滤波仿真为了更好地理解和掌握MATLAB中的卡尔曼滤波工具箱，我们将通过一个简单的实例演示其用法。
卡尔曼滤波原理及应用matlab仿真
卡尔曼滤波原理及应用matlab仿真卡尔曼滤波(Kalman Filter)是一种最优估计算法，由美国工程师卡尔曼发明并命名。
它是一种递归算法，适用于线性以及线性化的系统。
卡尔曼滤波可以通过已知的状态方程和观测方程来计算未知的状态量，同时考虑到测量误差和系统噪声。
卡尔曼滤波的核心思想是通过已知的状态方程和观测方程来递归地更新估计值和协方差矩阵。
估计值是对状态量的估计，协方差矩阵是表示估计值的不确定性的指标，它受到测量误差和系统噪声的影响。
通过不断迭代的过程，最终得到最优的状态估计值。
卡尔曼滤波主要应用于控制系统、导航、信号处理、图像处理等领域，它可以用于预测未来的状态量和优化估计结果，提高系统的稳定性和精度。
在自主导航系统中，卡尔曼滤波可以通过传感器捕捉环境信息，实现机器人的定位、控制和路径规划。
Matlab是一种强大的数学计算软件，它提供了丰富的工具箱和函数库，可以实现卡尔曼滤波算法的仿真。
Matlab中的Kalman滤波工具箱可以用于模拟线性系统的状态估计。
通过Matlab软件，可以输入系统的状态方程和观测方程，生成真实值和观测值序列，并使用卡尔曼滤波算法估计状态量，同时展示状态量的收敛过程和误差分析。
在实际应用中，卡尔曼滤波需要针对具体的问题进行调整和优化，例如选择不同的观测量和噪声模型，选择恰当的卡尔曼增益等。
因此，在使用卡尔曼滤波进行估计时需要注意以下几点：1.确定系统的状态方程和观测方程，建立合理的模型。
2.合理估计系统噪声和观测噪声，减小误差对估计结果的影响。
3.选择合适的卡尔曼增益，平衡观测值和实际值对估计的贡献。
4.对估计结果进行误差分析，评估卡尔曼滤波的优势和局限性。
总之，卡尔曼滤波是一种重要的最优估计算法，广泛应用于控制、导航、信号处理等领域。
通过Matlab软件，可以进行卡尔曼滤波算法的仿真，并优化估计结果。
在实际应用中，需要针对具体问题进行调整和优化，以提高估计精度和稳定性。
卡尔曼滤波器及matlab代码
信息融合大作业——维纳最速下降法滤波器，卡尔曼滤波器设计及Matlab仿真时间：2010-12-6专业：信息工程班级：09030702学号：71姓名：马志强1.滤波问题浅谈估计器或滤波器这一术语通常用来称呼一个系统，设计这样的系统是为了从含有噪声的数据中提取人们感兴趣的，接近规定质量的信息。
由于这样一个宽目标，估计理论应用于诸如通信、雷达、声纳、导航、地震学、生物医学工程、金融工程等众多不同的领域。
例如，考虑一个数字通信系统，其基本形式由发射机、信道和接收机连接组成。
发射机的作用是把数字源(例如计算机)产生的0、1符号序列组成的消息信号变换成为适合于信道上传送的波形。
而由于符号间干扰和噪声的存在，信道输出端收到的信号是含有噪声的或失真的发送信号。
接收机的作用是，操作接收信号并把原消息信号的一个可靠估值传递给系统输出端的某个用户。
随着通信系统复杂度的提高，对原消息信号的还原成为通信系统中最为重要的环节，而噪声是接收端需要排除的最主要的干扰，人们也设计出了针对各种不同条件应用的滤波器，其中最速下降算法是一种古老的最优化技术，而卡尔曼滤波器随着应用条件的精简成为了普适性的高效滤波器。
2．维纳最速下降算法滤波器最速下降算法的基本思想考虑一个代价函数，它是某个未知向量的连续可微分函数。
函数将的元素映射为实数。
这里，我们要寻找一个最优解。
使它满足如下条件这也是无约束最优化的数学表示。
特别适合于自适应滤波的一类无约束最优化算法基于局部迭代下降的算法：从某一初始猜想出发，产生一系列权向量，使得代价函数在算法的每一次迭代都是下降的，即其中是权向量的过去值，而是其更新值。
我们希望算法最终收敛到最优值。
迭代下降的一种简单形式是最速下降法，该方法是沿最速下降方向连续调整权向量。
为方便起见，我们将梯度向量表示为因此，最速下降法可以表示为其中代表进程，是正常数，称为步长参数，1/2因子的引入是为了数学上处理方便。
在从到的迭代中，权向量的调整量为为了证明最速下降算法满足式，在处进行一阶泰勒展开，得到此式对于较小时是成立的。
卡尔曼滤波器及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的高斯分布。
卡尔曼滤波 matlab算法
卡尔曼滤波 matlab算法卡尔曼滤波是一种用于状态估计的数学方法，它通过将系统的动态模型和测量数据进行融合，可以有效地估计出系统的状态。
在Matlab中，实现卡尔曼滤波算法可以通过以下步骤进行：1. 确定系统的动态模型，首先需要将系统的动态模型表示为状态空间方程，包括状态转移矩阵、控制输入矩阵和过程噪声的协方差矩阵。
2. 初始化卡尔曼滤波器，在Matlab中，可以使用“kf = kalmanfilter(StateTransitionModel, MeasurementModel, ProcessNoise, MeasurementNoise, InitialState, 'State', InitialCovariance)”来初始化一个卡尔曼滤波器对象。
其中StateTransitionModel和MeasurementModel分别是状态转移模型和测量模型，ProcessNoise和MeasurementNoise是过程噪声和测量噪声的协方差矩阵，InitialState是初始状态向量，InitialCovariance是初始状态协方差矩阵。
3. 进行预测和更新，在每个时间步，通过调用“predict”和“correct”方法，可以对状态进行预测和更新，得到最优的状态估计值。
4. 实时应用，将测量数据输入到卡尔曼滤波器中，实时获取系统的状态估计值。
需要注意的是，在实际应用中，还需要考虑卡尔曼滤波器的参数调节、性能评估以及对不确定性的处理等问题。
此外，Matlab提供了丰富的工具箱和函数，可以帮助用户更便捷地实现和应用卡尔曼滤波算法。
总的来说，实现卡尔曼滤波算法需要对系统建模和Matlab编程有一定的了解和技能。
希望以上内容能够对你有所帮助。
MATLAB中的Kalman滤波器设计
MATLAB中的Kalman滤波器设计引言Kalman滤波器是一种常用于估计随时间演变的系统状态的算法。
它通过观测值和系统模型之间的协方差来融合测量和预测，从而提供对系统状态的最优估计。
在MATLAB中，实现Kalman滤波器设计相对简单，本文将介绍MATLAB中Kalman滤波器的基本原理和常见的应用案例。
一、 Kalman滤波器基本原理Kalman滤波器的基本原理可以概括为两个步骤：预测和更新。
预测步骤根据系统的动力学模型和先前的状态估计，使用预测方程计算下一个时间步的状态预测和协方差预测。
更新步骤则是在测量信息的基础上，使用更新方程将预测的状态和协方差修正为更准确的估计。
预测步骤：1. 根据系统的动力学模型，使用预测方程计算状态预测值x_prior和协方差预测值P_prior：x_prior = F * x_est + B * uP_prior = F * P_est * F' + Q其中，F是状态转移矩阵，x_est和P_est是先前时间步的状态估计和协方差估计，B是控制输入矩阵，u是控制输入向量，Q是系统过程噪声的协方差矩阵。
更新步骤：1. 根据观测模型，使用更新方程计算观测预测值z_prior和观测协方差S：z_prior = H * x_priorS = H * P_prior * H' + R其中，H是观测矩阵，R是观测噪声的协方差矩阵。
2. 计算卡尔曼增益K：K = P_prior * H' * inv(S)3. 根据观测值z和观测预测值z_prior，计算状态更新值x_update和协方差更新值P_update：x_update = x_prior + K * (z - z_prior)P_update = (eye(size(x_est, 1)) - K * H) * P_prior二、案例研究：目标跟踪Kalman滤波器在目标跟踪领域有广泛的应用。
卡尔曼滤波算法及MATLAB实现
卡尔曼滤波算法及MATLAB实现这一段时间对现代滤波进行了学习，对自适应滤波器和卡尔曼滤波器有了一定认识，并对它们用MATLAB对语音信号进行了滤波，发现卡尔曼滤波器还是比较有用，能够在较大的噪声中还原原来的信号。
新的学期马上就开始了，由于TI的开发板一直在维修，所以学习TI开发板的计划搁置，但是对声音信号的处理及滤波器的认识有了进一步提高。
新的学期继续努力！卡尔曼滤波的基本思想是：以最小均方误差为最佳估计准则，采用信号与噪声的状态空间模型，利用前一时刻的估计值和当前时刻的观测值来更新对状态变量的估计，求出当前时刻的估计值，算法根据建立的系统方程和观测方程对需要处理的信号做出满足最小均方误差的估计。
语音信号在较长时间内是非平稳的，但在较短的时间内的一阶统计量和二阶统计量近似为常量，因此语音信号在相对较短的时间内可以看成白噪声激励以线性时不变系统得到的稳态输出。
假定语音信号可看成由一AR模型产生：时间更新方程：测量更新方程：K(t)为卡尔曼增益，其计算公式为：其中、分别为过程模型噪声协方差和测量模型噪声协方差，测量协方差可以通过观测得到，则较难确定，在本实验中则通过与两者比较得到。
由于语音信号短时平稳，因此在进行卡尔曼滤波之前对信号进行分帧加窗操作，在滤波之后对处理得到的信号进行合帧，这里选取帧长为256，而帧重叠个数为128；下图为原声音信号与加噪声后的信号以及声音信号与经卡尔曼滤波处理后的信号：原声音信号与加噪声后的信号原声音信号与经卡尔曼滤波处理后的信号MATLAB程序实现如下：%%%%%%%%%%%%%%%%%基于LPC全极点模型的最大后验概率估计法,采用卡尔曼滤波%%%%%%%%%%%%%% clear;clc;%%%%%%%%%%%%%%%%%%%%%%%%%%%加载声音数据%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%load voice.maty=m1(2,:);x=y+0.08*randn(1,length(y));%%%%%%%%%%%%%%%原声音信号和加噪声后的信号%%%%%%%%%%%%%%% figure(1);subplot(211);plot(m1(1,:),m1(2,:));xlabel('时间');ylabel('幅度');title('原声音信号');subplot(212);plot(m1(1,:),x);xlabel('时间');ylabel('幅度');title('加噪声后的信号'); %%%%%%%%%%%%%%%%%%%%%%%%%输入参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Fs=44100; %信号采样的频率bits=16; %信号采样的位数N=256; %帧长m=N/2; %每帧移动的距离lenth=length(x); %输入信号的长度count=floor(lenth/m)-1; %处理整个信号需要移动的帧数%%%先不考虑补零的问题p=11; %AR模型的阶数a=zeros(1,p);w=hamming(N); %加汉明窗函数y_temp=0;F=zeros(11,11); %转移矩阵F(1,2)=1;F(2,3)=1;F(3,4)=1;F(4,5)=1;F(5,6)=1;F(6,7)=1;F(7,8)=1;F(8,9)=1;F(9,10)=1;F(10,11)=1;H=zeros(1,p); %S0=zeros(p,1);P0=zeros(p);S=zeros(p);H(11)=1;s=zeros(N,1);G=H';P=zeros(p);%%%%%%%%%%%%%%%%测试噪声协方差%%%%%%%%%%%%%%%%%%%%%%y_temp=cov(x(1:7680));x_frame=zeros(256,1);x_frame1=zeros(256,1);T=zeros(lenth,1);for r=1:count%%%%%%%%%%%%%%%%%%%5%%%%%分帧处理%%%%%%%%%%%%%%%%%%%%% x_frame=x((r-1)*m+1:(r+1)*m);%%%%%%%%%%%%%%%%采用LPC模型求转移矩阵参数%%%%%%%%%%%%%%if r==1[a,VS]=lpc(x_frame(:),p);else[a,VS]=lpc(T((r-2)*m+1:(r-2)*m+256),p);end%%%%%%%%%%%%%%%%帧长内过程噪声协方差%%%%%%%%%%%%%%%%%%if (VS-y_temp>0)VS=VS-y_temp;elseVS=0.0005;endF(p,:)=-1*a(p+1:-1:2);for j=1:256if(j==1)S=F*S0;Pn=F*P*F'+G*VS*G';elseS=F*S; %时间更新方程Pn=F*P*F'+G*VS*G';endK=Pn*H'*(y_temp+H*P*H').^(-1); %卡尔曼增益P=(eye(p)-K*H)*Pn; %测量更新方程S=S+K*[x_frame(j)-H*S];T((r-1)*m+j)=H*S;end%%%%%%%%%%%%%%%%对得到的每帧数据进行加窗操作%%%%%%%%%%%%%%%%%%%%%%%%ss(1:256,r)=T((r-1)*m+1:(r-1)*m+256);sss(1:256,r)=ss(1:256,r).*w;end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%合帧操作%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% for r=1:countif r==1s_out(1:128)=sss(1:128,r);else if r==counts_out(r*m+1:r*m+m)=sss(129:256,r);elses_out(((r-1)*m+1):((r-1)*m+m))=sss(129:256,r-1)+sss(1:128,r);endendendfigure(2)subplot(211);plot(m1(1,:),m1(2,:));xlabel('时间');ylabel('幅度');title('原声音信号');subplot(212);plot((1:1109760)/Fs,s_out);xlabel('时间');ylabel('幅度');title('经卡尔曼滤波后的声音信号');。
MATLAB_对卡尔曼滤波器的仿真实现
XXXXX学院《电子信息系统仿真》课程设计届电子信息工程专业班级题目卡尔曼滤波器的设计与仿真姓名学号指导教师职称二О1年月日MATLAB 对卡尔曼滤波器的仿真实现课程设计目的：曼滤波器原理为理论基础，用MATLAB进行卡尔曼滤波器仿真、对比卡尔曼滤波器的预测效果，对影响滤波其效果的各方面原因进行讨论和比较，按照理论模型进行仿真编程，清晰地表述了编程过程。
关键词：数字信号处理；卡尔曼滤波器；MATLAB；仿真过程2.卡尔曼滤波基本原理卡尔曼滤波过程实际上是获取维纳解的递推运算过程。
从维纳解导出的卡尔曼滤波器实际上是卡尔曼滤波过程结束后达到稳态的情况，这时Kalman Filtering的结果与Wiener我们总结出卡尔曼的五个核心方程：X(k|k-1)=A X(k-1|k-1)+B U(k) (1)P(k|k-1)=A P(k-1|k-1) A’+Q (2)X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) (3)Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) (4)P(k|k)=（I-Kg(k) H）P(k|k-1) (5)计算滤波估计的流程图如图所示：图（1）可以看出，滤波过程是以不断地“预测—修正”的递推方式进行计算，先进行预测值计算，再根据观测值得到的新信息和kalman 增益（加权项），对预测值进行修正。
由滤波值可以得到预测，又由预测可以得到滤波，其滤波和预测相互作用，并不要求存储任何观测数据，可以进行实时处理。
3.程序设计卡尔曼滤波器给出了一个应用状态变量概念的公式。
而且，不同于其他的递归滤波器结构，它只需要记住一步的估计结果。
考虑过程噪声和测量噪声两个随机变量的状态模型称为随机状态模型。
用下面两个方程描述离散状态模型：1）过程方程：x(k +1) = Ax(k) + Bu(k) + w(k)其中，w(k)是由于过程模型的不确定性而产生的模型噪声，它可能是最难量化的参数。
卡尔曼滤波原理及应用-matlab仿真代码
一、概述在信号处理和控制系统中，滤波是一种重要的技术手段。
卡尔曼滤波作为一种优秀的滤波算法，在众多领域中得到了广泛的应用。
其原理简单而高效，能够很好地处理系统的状态估计和信号滤波问题。
本文将对卡尔曼滤波的原理及其在matlab中的仿真代码进行介绍，以期为相关领域的研究者和工程师提供一些参考和帮助。
二、卡尔曼滤波原理1.卡尔曼滤波的基本思想卡尔曼滤波是一种递归自适应的滤波算法，其基本思想是利用系统的动态模型和实际测量值来进行状态估计。
在每次测量值到来时，根据当前的状态估计值和测量值，通过递推的方式得到下一时刻的状态估计值，从而实现动态的状态估计和信号滤波。
2.卡尔曼滤波的数学模型假设系统的状态方程和观测方程分别为：状态方程：x(k+1) = Ax(k) + Bu(k) + w(k)观测方程：y(k) = Cx(k) + v(k)其中，x(k)为系统的状态向量，u(k)为系统的输入向量，w(k)和v(k)分别为状态方程和观测方程的噪声向量。
A、B、C为系统的参数矩阵。
3.卡尔曼滤波的步骤卡尔曼滤波的具体步骤如下：（1）初始化首先对系统的状态向量和协方差矩阵进行初始化，即给定初始的状态估计值和误差协方差矩阵。
（2）预测根据系统的状态方程，利用上一时刻的状态估计值和协方差矩阵进行状态的预测，得到状态的先验估计值和先验协方差矩阵。
（3）更新利用当前时刻的观测值和预测得到的先验估计值，通过卡尔曼增益计算出状态的后验估计值和后验协方差矩阵，从而完成状态的更新。
三、卡尔曼滤波在matlab中的仿真代码下面是卡尔曼滤波在matlab中的仿真代码，以一维线性动态系统为例进行演示。
定义系统参数A = 1; 状态转移矩阵C = 1; 观测矩阵Q = 0.1; 状态方程噪声方差R = 1; 观测噪声方差x0 = 0; 初始状态估计值P0 = 1; 初始状态估计误差协方差生成系统数据T = 100; 时间步数x_true = zeros(T, 1); 真实状态值y = zeros(T, 1); 观测值x_est = zeros(T, 1); 状态估计值P = zeros(T, 1); 状态估计误差协方差初始化x_est(1) = x0;P(1) = P0;模拟系统动态for k = 2:Tx_true(k) = A * x_true(k-1) + sqrt(Q) * randn(); 生成真实状态值y(k) = C * x_true(k) + sqrt(R) * randn(); 生成观测值预测x_pred = A * x_est(k-1);P_pred = A * P(k-1) * A' + Q;更新K = P_pred * C' / (C * P_pred * C' + R);x_est(k) = x_pred + K * (y(k) - C * x_pred);P(k) = (1 - K * C) * P_pred;end绘制结果figure;plot(1:T, x_true, 'b', 1:T, y, 'r', 1:T, x_est, 'g');legend('真实状态值', '观测值', '状态估计值');通过上面的matlab代码可以实现一维线性动态系统的状态估计和滤波，并且绘制出真实状态值、观测值和状态估计值随时间变化的曲线。
卡尔曼滤波器的MATLAB仿真实现
在 处理 一个离散控制 系统 的时候 ， 将该 系统进行数 学模
统：
两者混合 的非线性和线性 系统 ，利用 Ｍａ ｔ ｌ ａ ｂ使用者 能够 简 型化 ，以一个 线性随 机微 分方程来模 型化 这个离 散控 制系
ｘ （ １ ｃ ） ＝Ｍ・ Ｘ （ ｋ 一１ ） ＋Ｎ・ ｕ （ １ 【 ） ＋Ｙ （ １ ０
在（ １ ） 和（ ２ ） 中， × （ ｋ ） 描述 的是 系统 处于 ｋ时刻 的状 态 ， 用
的变化 ， 基本上是 由信号 源产生 的噪声 。例 如 ： 频 率合成器 ， Ｕ（ ｋ ） 来表示这个Ｂ － ， ｔ ｘ ＃ 怕 系统控制量。用两个参 数矩 阵来描述
节 目源 ， 电源等 ； 另一种是 “ 随机 ” 的变化 ， 一般 由电子 元器件 系统 的参数具体 为 ： Ｍ 和 Ｎ。这个离散控 制系统 的测量值用
系统 的量 测 值 ：
（ １ ）
的稳定运行和播 出效果。对广播发送设备 自身产生的噪声进 行检测和抑制是 日常维护工作 中一个重要 的部 分。在广播发
ｗ（ ｋ ） ＝Ｈ－ ｘ （ ｋ ） ＋ｚ （ ｋ ）
（ ２ ）
送设备运 行中产生的噪声一般 有两种情况 ， 一种是 “ 可 判定 ”
波 器 在 使 用过 程 中 具 有 一定 的局 限 性 。卡 尔 曼滤 波 器 不 同于 入 手 ， 在 基 于 其 原 理 的基 础 上 详 细 介 绍 卡 尔 曼 滤 波 器 Ｍａ ｔ ｌ ａ ｂ
最佳 滤波器 ， 其特 点可 以概括 为 ： 采用递推 的滤波 方法 ， 使 用 仿真 的具体 实现。为卡 尔曼滤波器在 实际维护 工作 中得 到更
卡尔曼滤波简介及其算法MATLAB实现代码 (1)
卡尔曼滤波简介说明及其算法MATLAB实现代码卡尔曼滤波算法实现代码（C，C＋＋分别实现）卡尔曼滤波器简介近来发现有些问题很多人都很感兴趣。
所以在这里希望能尽自己能力跟大家讨论一些力所能及的算法。
现在先讨论一下卡尔曼滤波器，如果时间和能力允许，我还希望能够写写其他的算法，例如遗传算法，傅立叶变换，数字滤波，神经网络，图像处理等等。
因为这里不能写复杂的数学公式，所以也只能形象的描述。
希望如果哪位是这方面的专家，欢迎讨论更正。
卡尔曼滤波器– Kalman Filter1．什么是卡尔曼滤波器（What is the Kalman Filter?）在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。
跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。
1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。
1957年于哥伦比亚大学获得博士学位。
我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。
如果对这编论文有兴趣，可以到这里的地址下载：.edu/~welch/media/pdf/Kalman1960.pdf。
简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。
对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。
他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。
近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。
MATLAB 对卡尔曼滤波的仿真实现
计算机实习报告MATLAB 对卡尔曼滤波的仿真实现MATLAB 对卡尔曼滤波的仿真实现实验目的基于卡尔曼滤波原理，利用matlab 针对以下模型编制代码进行仿真225(1x(k)=0.5x(k-1)+8cos[1.2(1)](1)1+x(k-1)()()()20x k k w k x k y k v k -+-+-=+）1：仿真100次，每次50步。
从100次仿真中任选一次，将其对应的状态真值和滤波结果曲线绘制在同一副图上，绘制时选用不同的线型，颜色并进行标注。
2：用均方根误差评价算法的精度，均方根误差定义为^1/2,,1()(()/)Mk j k j j REMS k x x M ==-∑M 为仿真次数，k 为离散时间点的索引值，,k j x 为第j 次仿真时第K 拍的真值。
请绘制出RMSE(k)随k 变化的曲线。
程序设计 程序代码如下：A=zeros(50,100);B=zeros(50,100);C=zeros(50,100); x=0; for i=1:1:100 for k=1:1:50x=0.5*x+25*x/(1+x^2)+8*cos(1.2*(i-1))+randn;A(k,i)=x;endendxk=0;P=1;for i=1:1:100for k=1:1:50xkk=0.5*xk+25*x/(1+xk^2)+8*cos(1.2*(k-1));F=1/2+25/(1+xk^2)-50*xk^2/(1+xk^2)^2;P=1+F*P*F;H=xk/10;S=H*P*H+10;K=P*H/S;xk=xkk+K*(xk^2/20+sqrt(10)*randn-xkk^2/20-sqrt(10)*randn);Pk=P-K*S*K;B(k,i)=xk;C(k,i)=Pk;endendD=A-B;Q=zeros(50,1);for k=1:1:50E=D(k,:);sum=0;for j=1:1:100sum=sum+E(j)^2;endRMSE=sqrt(sum/100);Q(k)=RMSE;endm=round(rand*100);subplot(1,2,1);plot(A(:,m),'rx-');hold onplot(B(:,m),'b*:');legend('真值','滤波');title('第一次仿真'); subplot(1,2,2); plot(Q);title('RMSE随k变化曲线');输出结果截屏显示为：程序运行中观测截屏程序运行中经过卡尔曼递推后估计值截屏程序运行中经过卡尔曼滤波后观测值与递推估计值比较截屏实验总结1.实验中遇到的问题(1).最初编写代码时将整个过程分为三步，第一步，对模型进行仿真，第二步，编写代码进行卡尔曼递推，第三步，对卡尔曼滤波后观测值与递推估计值比较。
卡尔曼滤波matlab仿真资料
随机信号处理上机作业题目：假设有一个二坐标雷达对一平面上运动目标的进行观察，目标在t ＝0～400秒沿y 轴作恒速直线运功，运动速度为-15m/s ，目标的起点为（2000m ，10000m ），雷达扫描周期为2秒，x 和y 独立地进行观察，观察噪声的标准差均为100m 。
试建立雷达对目标的跟踪算法，并进行仿真分析，给出仿真结果，画出目标真实轨迹、对目标的观察和滤波曲线。
一、跟踪算法考虑利用卡尔曼滤波算法对目标的运动状态进行估计。
由于目标在二维平面内做匀速运动，因此这里只考虑匀速运动情况。
1. 建立模型由于目标沿y 轴作匀速直线运动，取状态变量⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=y v y x S 状态方程：()()k AS k S =+1 (1)观测方程：()()()k V k CS k Z += (2)其中，⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=10010001T A ⎥⎦⎤⎢⎣⎡=010001C⎥⎦⎤⎢⎣⎡=y x z z Z ⎥⎦⎤⎢⎣⎡=y x v v V对目标位置和速度的同时滤波与一步预测的方程组如下:预测估计方程:()()11/^^-=-k A k k S S预测误差协方差：()()T A k AP k k P 11/-=-滤波估计增益：()()()R C k k C k k C k B T T+--=1/1/，其中⎥⎥⎦⎤⎢⎢⎣⎡=2200y x R σσ 滤波估计方程：()()()()()⎥⎦⎤⎢⎣⎡--+-=1/1//^^^k k k Z k B k k k k S S S滤波误差协方差：()()[]()1/1/--=k k P C k B k k P2. 初始化利用目标的前几个测量值建立状态的其实估计,采用两点起始法。
()⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡-=∧T Z Z Z Z S y y y x )1()2()2()2(2/2()⎥⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎢⎣⎡=T T T P y y y yx 22222200002/2σσσσσ滤波误差均值：()k k S k S N k e I Ni i x /)(1)(1∧=-=∑滤波误差标准差：()221)(/)(1k e k k S k S Nx Ni I i y-⎥⎦⎤⎢⎣⎡-=∑=∧∧σ二. 仿真分析利用MATLAB 对前面建立的模型进行仿真,结果如下。
卡尔曼滤波原理及应用matlab
卡尔曼滤波原理及应用matlab卡尔曼滤波是一种最优线性滤波算法，经常应用于估计系统的状态，特别是在有观测误差和系统动态噪声的情况下。
它是由卡尔曼于1960年提出的，常用于航天、制导导航控制等领域。
卡尔曼滤波的核心思想是通过将系统的测量值与模型预测值进行加权平均，得到对系统状态的最优估计。
它的主要特点是能够自适应地估计系统的状态，并且对于含有噪声的测量值具有较好的抗扰动能力。
在卡尔曼滤波中，系统的状态通常用状态向量表示，用一个线性方程组表示系统的动态演化，如下所示：x(k) = A * x(k-1) + B * u(k-1) + w(k-1)其中，x(k)表示系统的状态向量，A和B是状态转移矩阵和输入控制矩阵，u(k-1)表示输入控制向量，w(k-1)表示系统动态噪声。
系统的测量模型可以表示为：z(k) = H * x(k) + v(k)其中，z(k)为测量值，H为测量矩阵，v(k)表示观测噪声。
卡尔曼滤波的基本步骤如下：1. 预测状态：根据上一时刻的状态估计值和状态转移矩阵进行预测，得到对当前状态的预测估计。
x^(k k-1) = A * x^(k-1 k-1) + B * u(k-1)P(k k-1) = A * P(k-1 k-1) * A' + Q(k-1)2. 更新观测：根据当前的测量值和测量模型进行更新，得到对当前状态的最优估计。
K(k) = P(k k-1) * H' * inv(H * P(k k-1) * H' + R(k))x^(k k) = x^(k k-1) + K(k) * (z(k) - H * x^(k k-1))P(k k) = (I - K(k) * H) * P(k k-1)3. 输出最优估计：使用更新后的状态估计和协方差矩阵作为当前时刻的最优估计结果。
x(k) = x^(k k)P(k) = P(k k)其中，P(k k-1)和P(k k)是协方差矩阵，Q(k-1)和R(k)是系统动态噪声和观测噪声的协方差矩阵。
卡尔曼滤波器的PID控制：kalman的PID控制教程(MATLAB优化算法案例分析应用PPT课件)
MATLAB优化算法案例分析与应用
•2 基于卡尔曼滤波器的PID控制
2.1 含噪音信号的滤波常见处理方法
（1）FIR 滤波器
重复100次，
0.11
平 均 MSE
0.105
0.1
0.095
0.09
0.085
0.08
0.075
0.07
0.065 0
10 20 30 40
50 60 70
80 90 100
x=A*x+Mn*(yv(k)-C*A*x);
ye(k)=C*x+D;
%滤波值
errcov(k)=C*P*C'; 值
%估计量协方差
%Time update x=A*x+B*u(k);
u_2=u_1;u_1=u(k); y_2=y_1;y_1=ye(k);
MATLAB优化算法案例分析与应用
•2 基于卡尔曼滤波器的PID控制
MATLAB优化算法案例分析与应用
•2 基于卡尔曼滤波器的PID控制
2.1 含噪音信号的滤波常见处理方法
卡尔曼滤波理论，该理论采用时域上的递推算法在数字计算机上进行 数据滤波处理。通过不断的更新和矫正协方差值，通过不断的获取系统测量 值，不断的把covariance递归，从而估算出最优估计值。Kalman滤波具有 实时性，通过测量跟踪实现信号的分析处理，较 LMS 滤波器和 FIR 滤波器 ，具有误差小、实时效果好、滤波平滑等特点，广泛应用于动态多变量系统 状态建模中。
图18 无滤波器时PID控制阶跃响应（M=1）
yd,yout
MATLAB优化算法案例分析与应用
•2 基于卡尔曼滤波器的PID控制
2.3 采用卡尔曼滤波PID控制
kalman滤波在不同信噪比时的误码率matlab仿真程序
-20-15-10-50510152000.050.10.150.20.250.30.350.40.450.5Kalman滤波器在matlab仿真程序下的不同信噪比时的误码率：%multiuser_dectect.mclc;clear all;hold onBER_sum=zeros(1,13);%设定求和误码率的零矩阵；BER_ave=zeros(1,13); %设定平均误码率的零矩阵；for m=1:10；%m的长度为1到10 间隔为1；snr_in_db=-20:3:16;%定义信噪比的长度为-20到16 间隔为3；snr_in_db是信噪比用db表示for i=1:length(snr_in_db);%i的长度为1到信噪比的长度BER(i)= Kalman_S1(snr_in_db(i));%卡尔曼的误码率函数；endBER_sum=BER_sum+BER;%误码率求和的算法end;BER_ave=0.1* BER_sum ; %误码率平均值的算法semilogy( snr_in_db,BER_ave,'rd-')；%y轴维数坐标图定义横坐标为信噪比，纵坐标为误码率；%Kalman_S1.m%Kalman algorithm%synchronous CDMA同步cdma%channel: White Gaussis Noisefunction [p] = Kalman_S1(snr_in_dB)SNR=10^(snr_in_dB/10); %信噪比由dB形式转化sgma=1; % noise standard deviation is fixed 定义方差Eb=sgma^2*SNR;A=[sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqr t(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb)];K=length(A);step=1000;%阶跃响应指令迭代1000次N=31;%扩频增益n=31%******************************************************************* *******S1=[1 1 1 1 1 -1 -1 -1 1 1 -1 1 1 1 -1 1 -1 1 -1 -1 -1 -1 1 -1 -1 1 -1 1 1 -1 -1];%a(n)=a(2)+a(-1)S2=[1 -1 -1 -1 -1 1 -1 1 1 -1 1 -1 1 -1 -1 -1 1 1 1 -1 1 1 1 1 1 -1 -1 1 -1 -1 1];%a(n)=a(3)+a(2)+a(1)+a(-1)for i=1:NSS(i,:)=(S1~=[S2(i:N),S2(1:i-1)]);endSS=SS*2-1;S=SS(1:K,:);%K*NC(1,:)=SS(1,:);C(2:N,:)=rand(N-1,N)*2-1;for i=2:Nrou=C(i,:)*C(1:i-1,:).'./dot(C(1:i-1,:),C(1:i-1,:),2).';C(i,:)=C(i,:)-rou*C(1:i-1,:);endfor i=1:NC(i,:)=C(i,:)/sqrt(dot(C(i,:),C(i,:)));endC_null=C(2:N,:);%********************************************b=zeros(K,step);b=sign(rand(K,step)-0.5);noiseN_step=randn(N,step); % NoisenoiseN_step=sqrt(10./SNR)*noiseN_step;for i=1:KAb(i,:)=A(i)*b(i,:);endyN_step=(Ab.'*S).';yN_step=yN_step+noiseN_step; %****************************** KK=eye(N-1);w=zeros(N-1,1);c=zeros(N,1);%Kalman multiuser detectionfor i=1:stepd=C_null*yN_step(:,i);g=KK*d*(d'*KK*d+1)^(-1);KK=KK-g*d'*KK;yy=S(1,:)*yN_step(:,i);w=w+g*(yy-d'*w);c=S(1,:).'-C_null.'*w;y(i)=sign(real(c.'*yN_step(:,i))); endp= length(find(y - b(1,:)))/length(y);。
基于Matlab的卡尔曼滤波算法仿真
基于Matlab的卡尔曼滤波算法仿真1.卡尔曼滤波器原理卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计值和最近一个观察数据来估计信号的当前值。
它是用状态方程和递推方法进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给出其信号模型是从状态方程和量测方程得到的。
卡尔曼滤波中信号和噪声是用状态方程和测量方程来表示的。
因此设计卡尔曼滤波器要求已知状态方程和测量方程。
它不需要知道全部过去的数据，采用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。
卡尔曼几个重要公式：ŝ(n|n) = a ŝ (n-1|n-1) + G n[x(n) – ac ŝ (n-1|n-1)] (1)P(n) = a2ξ(n-1) + Q (2)G n = (3)ξ(n) = = (1 – cG n)P(n) (4)这组方程的递推计算过程如图1所示。
图1. 卡尔曼滤波器递推运算流程图-卡尔曼滤波过程实际上是获取维纳解的递推运算过程，这一过程从某个初始状态启动，经过迭代运算，最终到达稳定状态，即维纳滤波状态。
递推计算按图1所示进行。
假设已经有了初始值ŝ(0|0)和ξ（0），这样便可由式（2）计算P(1)，由式（3）计算G1，由式（4）计算ξ（1），由式（1）计算ŝ(1|1)。
ξ（1）和ŝ(1|1)便成为下一轮迭代运算的已知数据。
在递推运算过程中，随着迭代次数n的增加，ξ（n）将逐渐下降，知道最终趋近于某个稳定值ξ0。
这时ξ（n）= ξ（n - 1）= ξ0为求得这个稳定值，将式（3）和式（2）代入式（4），得到ξ02 +解此方程即可求出ξ0。
2.基于Matlab的卡尔曼滤波器的仿真Matlab代码如下：clearN=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);endfor k=1:N;Y(k)=c*x(k)+Q2(k);endp(1)=10;s(1)=1;for t=2:N;Rww = cov(Q1(1:t));Rvv = cov(Q2(1:t));- p1(t)=a.^2*p(t-1)+Rww;%kalman 增益b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));p(t)=p1(t)-c*b(t)*p1(t);endFontSize=14;LineWidth=3;figure();%画出温度计的测量值plot(Y,'k+');hold on;%画出最优估计值plot(s,'b-','LineWidth',LineWidth)hold on;%画出真实值plot(x,'g-','LineWidth',LineWidth);legend('观测值', '后验估计', '真实值');xl=xlabel('');yl=ylabel('');set(xl,'fontsize',FontSize);set(yl,'fontsize',FontSize);hold off;set(gca,'FontSize',FontSize);figure();valid_iter = [2:N];%画出最优估计值的方差plot(valid_iter,p([valid_iter]),'LineWidth',LineWidth);legend('后验估计的误差估计');xl=xlabel('');yl=ylabel('');set(xl,'fontsize',FontSize);set(yl,'fontsize',FontSize);set(gca,'FontSize',FontSize);-Matlab仿真结果如下：卡尔曼滤波的结果：估计的误差的方差：-卡尔曼滤波的实质是由测量值重构系统的状态向量。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
-20-15-10-50510152000.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
0.45
0.5
Kalman滤波器在matlab仿真程序下的不同信噪比时的误码率：
%multiuser_dectect.m
clc;
clear all;
hold on
BER_sum=zeros(1,13);%设定求和误码率的零矩阵；
BER_ave=zeros(1,13); %设定平均误码率的零矩阵；
for m=1:10；%m的长度为1到10 间隔为1；
snr_in_db=-20:3:16;%定义信噪比的长度为-20到16 间隔为3；snr_in_db是信噪比用db表示
for i=1:length(snr_in_db);%i的长度为1到信噪比的长度
BER(i)= Kalman_S1(snr_in_db(i));%卡尔曼的误码率函数；
end
BER_sum=BER_sum+BER;%误码率求和的算法
end;
BER_ave=0.1* BER_sum ; %误码率平均值的算法
semilogy( snr_in_db,BER_ave,'rd-')；%y轴维数坐标图定义横坐标为信噪比，纵坐标为误码率；
%Kalman_S1.m
%Kalman algorithm
%synchronous CDMA同步cdma
%channel: White Gaussis Noise
function [p] = Kalman_S1(snr_in_dB)
SNR=10^(snr_in_dB/10); %信噪比由dB形式转化
sgma=1; % noise standard deviation is fixed 定义方差
Eb=sgma^2*SNR;
A=[sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqr t(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb),sqrt(Eb)];
K=length(A);
step=1000;%阶跃响应指令迭代1000次
N=31;%扩频增益n=31
%******************************************************************* *******
S1=[1 1 1 1 1 -1 -1 -1 1 1 -1 1 1 1 -1 1 -1 1 -1 -1 -1 -1 1 -1 -1 1 -1 1 1 -1 -1];%a(n)=a(2)+a(-1)
S2=[1 -1 -1 -1 -1 1 -1 1 1 -1 1 -1 1 -1 -1 -1 1 1 1 -1 1 1 1 1 1 -1 -1 1 -1 -1 1];%a(n)=a(3)+a(2)+a(1)+a(-1)
for i=1:N
SS(i,:)=(S1~=[S2(i:N),S2(1:i-1)]);
end
SS=SS*2-1;
S=SS(1:K,:);%K*N
C(1,:)=SS(1,:);
C(2:N,:)=rand(N-1,N)*2-1;
for i=2:N
rou=C(i,:)*C(1:i-1,:).'./dot(C(1:i-1,:),C(1:i-1,:),2).';
C(i,:)=C(i,:)-rou*C(1:i-1,:);
end
for i=1:N
C(i,:)=C(i,:)/sqrt(dot(C(i,:),C(i,:)));
end
C_null=C(2:N,:);
%********************************************
b=zeros(K,step);
b=sign(rand(K,step)-0.5);
noiseN_step=randn(N,step); % Noise
noiseN_step=sqrt(10./SNR)*noiseN_step;
for i=1:K
Ab(i,:)=A(i)*b(i,:);
end
yN_step=(Ab.'*S).';
yN_step=yN_step+noiseN_step; %****************************** KK=eye(N-1);
w=zeros(N-1,1);
c=zeros(N,1);
%Kalman multiuser detection
for i=1:step
d=C_null*yN_step(:,i);
g=KK*d*(d'*KK*d+1)^(-1);
KK=KK-g*d'*KK;
yy=S(1,:)*yN_step(:,i);
w=w+g*(yy-d'*w);
c=S(1,:).'-C_null.'*w;
y(i)=sign(real(c.'*yN_step(:,i))); end
p= length(find(y - b(1,:)))/length(y);。