二维singer滤波

  • s3_725656
    了解作者
  • matlab
    开发工具
  • 1.3KB
    文件大小
  • rar
    文件格式
  • 0
    收藏次数
  • VIP专享
    资源类型
  • 0
    下载次数
  • 2022-02-22 10:15
    上传日期
二维卡尔曼滤波,使用singer模型。 这是目标作机动时,卡尔曼滤波模型采用singer模型的matlab 程序.
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的...