# 二维singer滤波

• s3_725656
了解作者
• matlab
开发工具
• 1.3KB
文件大小
• rar
文件格式
• 0
收藏次数
• VIP专享
资源类型
• 0
下载次数
• 2022-02-22 10:15
上传日期

Singer.rar
• main.m
2.3KB
• track2D.m
531B

clc;close all;clear; T = 2; %采样周期 % sigmaQ = 100; sigmaR = 200; N = 100; F=[1 T T^2/2 0 0 0; 0 1 T 0 0 0; 0 0 1 0 0 0; 0 0 0 1 T T^2/2; 0 0 0 0 1 T; 0 0 0 0 0 1];%状态转移矩阵 H = [1 0 0 0 0 0; 0 0 0 1 0 0];%量测矩阵 R = [sigmaR^2 0; 0 sigmaR^2];%量测协方差 qq11 = T^5/20; qq12 = T^4/8; qq13 = T^3/6; qq22 = T^3/3; qq23 = T^2/2; qq33 = T; Q=[qq11 qq12 qq13 0 0 0; qq12 qq22 qq23 0 0 0; qq13 qq23 qq33 0 0 0; 0 0 0 qq11 qq12 qq13; 0 0 0 qq12 qq22 qq23; 0 0 0 qq13 qq23 qq33];%过程噪声协方差 %初始化跟踪曲线 X = track2D(0.1,N); X = [X(1:2,:);0.1*randn(1,N);X(3:4,:);0.14*randn(1,N)]; Z = [X(1,:);X(4,:)]; % Z = X; % X = reshape(X,6,1,100); % %直角坐标转极坐标 % for i = 1:N % d = sqrt(X(1,i)^2+X(4,i)^2); % sita = atan(X(4,i)/X(1,i)); % end % %极坐标测量误差 % d = d + sigmaR*randn(1,N); % sita = sita + 0.001*randn(1,N); % %极坐标转直角坐标 % for i = 1:N % Z(1,i) = d(1,i)*cos(sita(1,i)); % Z(2,i) = d(1,i)*sin(sita(1,i)); % end X = reshape(X,6,1,N); Z = Z + sigmaR*randn(2,N); Z = reshape(Z,2,1,N);%测量数据 for i = 1:N xtr(i) = Z(1,1,i); ytr(i) = Z(2,1,i); end plot(xtr,ytr,'.-'); % xe(:,:,1) = Z(:,:,1); xe(:,:,2) = [Z(1,1,2),(Z(1,1,2)-Z(1,1,1))/T,2*(Z(1,1,2)-Z(1,1,1))/T^2,... Z(2,1,2),(Z(2,1,2)-Z(2,1,1))/T,2*(Z(2,1,2)-Z(2,1,1))/T^2]'; xp(:,:,1) = zeros(6,1); xp(:,:,2) = zeros(6,1); %滤波协方差初始化 P11=R(1,1); P12=R(1,1)/T; P13=R(1,1)/T^2; P22=2*R(1,1)/T^2; P23=3*R(1,1)/T^3; P33=6*R(1,1)/T^4; P44=R(2,2); P45=R(2,2)/T; P46=R(2,2)/T^2; P55=2*R(2,2)/T^2; P56=3*R(2,2)/T^3; P66=6*R(2,2)/T^4; p(:,:,2)=[P11 P12 P13 0 0 0; P12 P22 P23 0 0 0; P13 P23 P33 0 0 0; 0 0 0 P44 P45 P46; 0 0 0 P45 P55 P56; 0 0 0 P46 P56 P66]; for k = 3:N xp(:,:,k) = F*xe(:,:,k-1); a(:,:,k) = Z(:,:,k)-H*xp(:,:,k);%新息 pt(:,:,k) = F*p(:,:,k-1)*F'+Q; A(:,:,k) = H*pt(:,:,k)*H'+R; K(:,:,k) = pt(:,:,k)*H'*inv(A(:,:,k)); xe(:,:,k) = xp(:,:,k)+K(:,:,k)*a(:,:,k); p(:,:,k) = (eye(6)-K(:,:,k)*H)*pt(:,:,k); end for i = 1:N; xtr(i) = xe(1,1,i); ytr(i) = xe(4,1,i); end plot(xtr,ytr,'.-'); legend('真轨迹','测量轨迹','跟踪轨迹');

相关推荐
• Matlab合集
冈萨雷斯数字图像处理MATLAB版.中文版+数字图像处理第二版中文版（冈萨雷斯）+MATLAB-R2014a完全自学一本通+MATLAB R2016a完全自学一本通 素材文件+[模式识别与智能计算：MATLAB技术实现(第2版)].杨淑莹.扫描版
• MATLAB教程
MATLAB教程MATLAB教程MATLAB教程MATLAB教程MATLAB教程MATLAB教程
• MATLAB
MATLAB 该项目是在matlab上完成的，涉及创建和移动宇宙飞船和机器人。 太空飞船和机器人是使用Matlab中的简单几何形状创建的，并通过连续变换矩阵进行移动。 这个项目教我如何使用变换矩阵（旋转，平移等）的概念...
• MATLAB基础
一本学习matlab的一本好书
• MATLAB编译器
基于MATLAB 2018b版本介绍MATLAB编译器。介绍如何利用编译器将MATLAB代码编译为独立应用程序或组件，并在没有安装MATLAB的计算机上进行部署。
• matlabruntime
通过奇点容器运行您的matlab项目 可以在没有MATLAB的容器中运行matlab代码。 为此，我们首先需要通过Matlab编译器在本地构建相应的Matlab代码的独立应用程序，然后使用具有Matlab运行时（ 的容器）运行该应用程序 ）...
• matlab实现
matlab实现 matlab实现matlab实现matlab实现matlab实现
• matlab 教程
matlab 信号处理资料，里面包含信号处理pdf文档，一些杂乱的程序和命令等
• matlab教程
matlab教程，PPT格式，包含 matlab基本知识、matlab入门、matlab作图、线性规划、无约束优化、非线性规划、统计工具箱、差值、微分方程等多项知识点，并且每个知识点独立成为PPT，内还含有matlab信号处理详解等文档...
• matlab简介
1.MATLAB 开发环境 1.1 MATLAB 的视窗环境 进入MATLAB之后，会看到一个视窗MATLAB Command Window称为指令视窗，它是你键入指令的地方同时 MATLAB也将计算结果显示在此。 1.2 简易计算 我们先从MATLAB的...