Kalman滤波锁相及Matlab仿真
来源:互联网 发布:数据分析师报考条件 编辑:程序博客网 时间:2024/06/01 13:35
采用卡尔曼滤波算法锁相
信号为正弦波,考虑幅值、谐波、频率变化的影响,给出Matlab仿真的S-Function函数
Kalman Filter原理
function [sys,x0,str,ts] = KalmanFilter(t,x,u,flag)% x(n+1) = Ax(n) + Bu(n)% y(n) = Cx(n) + Du(n) % See sfuntmpl.m for a general S-function template.f1 = 50;Ts = 0.0001;A = [cos(2*pi*f1*Ts) -sin(2*pi*f1*Ts) sin(2*pi*f1*Ts) cos(2*pi*f1*Ts)];H = [1 0];R = 1;Q = [0.001 0;0 0.001]; %2*2%Q = [0.011 0; 0 0.011]*A*A;%R = 20*A*A;X0= [0;-2];P0 = [0;0;0;0]; %4*1 -> 2*2I = [1 0;0 1];switch flag, case 0, [sys,x0,str,ts] = mdlInitializeSizes(t,X0,P0); case 2, sys = mdlUpdate(t,x,u,A,Q,R,H,I); case 3, sys = mdlOutputs(t,x); case 9, sys = []; % do nothing otherwise DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));endfunction [sys,x0,str,ts] = mdlInitializeSizes(t,X0,P0)sizes = simsizes;sizes.NumContStates = 0;sizes.NumDiscStates = 6;sizes.NumOutputs = 4; %sinTheta cosTheta cosTheta magnitudesizes.NumInputs = 1; %Ua_measured = Z <- usizes.DirFeedthrough = 1;sizes.NumSampleTimes = 1;sys = simsizes(sizes);x0 = [X0;P0]; %collumstr = [];ts = [0.0001 0];function sys = mdlUpdate(t,x,u,A,Q,R,H,I)Z = u; %input Ua_measuredX1 = [x(1);x(2)]; %state X 2*1P1 = [x(3) x(4);x(5) x(6)]; %state P 2*2Xmid = A*X1; %mid X 2*1Pmid = A*P1*A.'+Q; %mid P 2*2K = Pmid*H.'/(H*Pmid*H.'+R); %2*1/number(1 dim)X2 = Xmid+K*(Z-H*Xmid); %update X 2*1P2 = (I-K*H)*Pmid; %update P 2*2sys = [X2(1);X2(2);P2(1,1);P2(1,2);P2(2,1);P2(2,2)];function sys = mdlOutputs(t,x,u)X = [x(1);x(2)];sinTheta = X(1,1);cosTheta = X(2,1);theta = atan2(sinTheta,-cosTheta);magnitude = hypot(sinTheta,cosTheta);sinTheta = sinTheta/magnitude;cosTheta = cosTheta/magnitude;sys = [sinTheta;cosTheta;theta;magnitude];
阅读全文
0 0
- Kalman滤波锁相及Matlab仿真
- Kalman滤波 Matlab仿真
- Matlab Kalman滤波
- kalman滤波的相关理解及MATLAB程序注解
- 卡尔曼滤波基本原理及matlab仿真
- matlab 实现的kalman滤波
- kalman滤波matlab代码demo
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-2.1
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-2.2
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-3.1
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-3.2
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-3.3
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-3.4
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-4.1
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-4.2
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-4.3
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-4.4
- 《卡尔曼滤波原理及应用-MATLAB仿真》程序-7.1
- P1427 小鱼的数字游戏
- 编译alsa-lib, alsa-util以使用声卡
- 如何在ubuntu下查看隐藏的文件和文件夹
- 游戏物理数学之《四元数Quaternion》
- node.js vs. Spring
- Kalman滤波锁相及Matlab仿真
- 递归算法——入门汉诺塔
- nginx配置文件详解
- Java Set集合使用方法介绍(2)——宠物猫信息管理
- 双硬盘+win7+ubuntu双系统 去掉从硬盘后ubuntu无法正常启动问题解决方法
- 基于邻接矩阵的图的深度和广度遍历
- mysql “ERROR 2003: Can't connect to MySQL server on 'localhost' (10061)”
- tomcat部署项目class文件丢失解决办法
- ibatis源码学习1_整体设计和核心流程