在设计卡尔曼滤波数据处理模块时.docVIP

在TWS雷达系统中，对跟踪目标的数据进行处理常常要进行卡尔曼滤波。卡尔曼滤波是一种最优估计的递推滤波算法，具有实时性好和精度高的特点。但是由于其算法中多为矩阵运算，所以较其它算法如最小二乘法、α－β算法等的运算量要大。为了减小运算量以满足系统反应时间的要求，应用中往往采用简化的卡尔曼滤波算法，但这样又会带来滤波精度的降低。在设计卡尔曼滤波数据处理模块时，为了解决该矛盾，采用了高速浮点型DSP TMS320C31。TMS320C31是美国TI公司的第三代DSP产品，其内部采用程序和数据分开的哈佛结构、流水线操作以及并发I/O和CPU操作。芯片内含有专用硬件乘法器和桶形移位寄存器，具有３２位的浮点精度，特别适合类似卡尔曼滤波这样运算量大、实时性和计算精度要求高的场合。 １ 卡尔曼滤波算法 卡尔曼滤波器的状态方程和量测方程如下：Ｘ(Ｋ＋１)＝Ａ(Ｋ)Ｘ(Ｋ)＋Ｗ(Ｋ)Ｚ(Ｋ)＝Ｈ(Ｋ)Ｘ(Ｋ)＋Ｖ(Ｋ)　 式中，Ｘ(K)＝[X1(K)，X2(K)，X3(K)，X4(K)]Ｔ Z(K)=[Z1(K)，Z2(K)]ＴＷ(K)＝[０，Ｕ１(K)，０，Ｕ２(K)]ＴＶ(K)＝[Ｖ１(K)，Ｖ２(K)]ＴＡ＝1T00 0100 001T 0001 Ｈ＝1000 0010Ｘ１(K)＝ρ(Ｋ)　Ｘ２(Ｋ)＝ρ(Ｋ)　Ｘ３(K)＝θ(K)　Ｘ４(K)＝θ(K)　Ｔ为采样周期ρ(K)，ρ(Ｋ)分别为距离和径向速度的估值θ(K)　、(K)分别为方位角和方位角速度的估值 Ｕ１(K)　表示Ｔ时间内径向速度的变化量Ｕ２(K)　表示Ｔ时间内方位角速度的变化量Ｖ１(K)为距离量测噪声Ｖ２(K)　为方位角量测噪声Ｚ１(K)　和Ｚ２(K)分别为距离和角度的测量值滤波器递推方程为：X１(Ｋ／Ｋ－１)＝Ａ(Ｋ－１／Ｋ－１)Ｐ１(Ｋ／Ｋ－１)＝ＡＰ(Ｋ－１／Ｋ－１)ＡＴ＋Ｑ(Ｋ－１)　Ｇ(Ｋ)＝Ｐ１(Ｋ／Ｋ－１)ＨＴ[ＨＰ１(Ｋ／Ｋ－１)ＨＴ＋Ｒ(Ｋ)]－１Ｐ(Ｋ／Ｋ)＝Ｐ(Ｋ／Ｋ－１)－Ｇ(Ｋ)ＨＰ１(Ｋ／Ｋ－１)X１(Ｋ／Ｋ)＝１(Ｋ／Ｋ－１)＋Ｇ(K)Ｚ(Ｋ)－Ｚ(Ｋ)Z(Ｋ)＝Ｈ１(Ｋ／Ｋ－１) 其中，Ｇ(K)为卡尔曼增益矩阵Ｑ(K)、Ｒ(K)分别为状态噪声协方差矩阵和量测噪声协方差矩阵Ｐ(K)为误差协方差矩阵２ 基于TMS320C31的数据处理模块的工作原理 ２．１ 多处理器工作模式下主机和TMS320C31数据通信的设计 数据处理模块由双口ＲＡＭ ＩＤＴ７１３４、TMS320C31(以下简称Ｃ３１)、ＥＥＲＯＭ ２８Ｆ０１０和高速ＲＡＭ ＣＹ７Ｃ１９９组成通过ＩＳＡ总线和主计算机连接起来，构成主从式多处理器工作模式，如图１所示。主从式处理器系统的设计关键在于处理器之间的数据交换，主从处理器间的数据交换主要有串行、并行、ＤＭＡ及双口ＲＡＭ四种交换方式。本方案采用ＩＤＴ公司的双口ＲＡＭ ＩＤＴ７１３４实现主计算机和Ｃ３１间的数据交换。ＩＤＴ７１３４内部含有４ＫＢ的存储器资源，具有两组地址、数据总线及读写控制线。主计算机和Ｃ３１可同时对其进行读写操作。在对其内部同一存储单元访问时，要考虑写入和读出数据的完整性。ＩＤＴ７１３４没有单独的主从处理器访问控制引脚，设计时需用软件方式来保证主计算机和Ｃ３１之间数据交换的正确性。本方案中采用了在ＩＤＴ７１３４中设置输入输出ｓｅｍａｐｈｏｒｅ的方案，即主计算机在将跟踪雷达的测量数据写入双口ＲＡＭ后，设置输入ｓｅｍａｐｈｏｒｅ，通知Ｃ３１读取测量数据。Ｃ３１进行卡尔曼滤波后，将滤波结果写入双口ＲＡＭ，设置输出ｓｅｍａｐｈｏｒｅ，通知主计算机读取。双口ＲＡＭ的数据宽度为８位，Ｃ３１数据总线宽度为３２位。双口ＲＡＭ的数据总线和Ｃ３１低８位数据总线相接。为了提高数据处理的精度，双口ＲＡＭ中的跟踪数据和Ｃ３１的滤波结果都以字(３２位)为单位进行存储，Ｃ３１作卡尔曼滤波时，首先按字节读取跟踪数据，然后进行移位，拼接成３２位整形数，再将整形数转换为浮点数，进行滤波运算。输出时，则将浮点型的结果转为整形数，按字节写入双口ＲＡＭ中。双口ＲＡＭ空间分配如图２所示。  ２．２ C31的系统装入和管理 C31有两种工作模式，即微处理器和微计算机方式，本方案中，Ｃ３１的ＭＣＢＬ／ＭＰ脚接高电平，Ｃ３１工作于微计算机方式。Ｃ３１具有程序引导功能，其内部固化有引导程序(ＢＯＯＴ)，可将来自其它处理器、低速ＥＰＲＯＭ或其它标准的存储器件的程序和数据装入到高速静态ＲＡＭ中以全速运行。Ｃ３１有两种装入方式可供用户选择：串行装入方式和存储器装入方式。存储器装入方式根据地址的不同又分为三种：ＢＯＯＴ１、ＢＯＯＴ２、ＢＯＯＴ３。Ｃ３１通过查询四个中断引脚确定装入