第11组赵魏郭泽宇李晓龙。
一、 题目。
雷达追踪目标的滤波算法（观测变量：距离信息和角度信息）
雷达是追踪和计算目标位置的重要设备，可以提供目标的相对距离和相对角度信息，如下图所示：
假设在当地水平坐标系上（x y z 分别指向北向、天向和东向）描述目标相对于雷达的位置和速度信息。雷达测量的视距、方位角、高低角的理论模型为：
试采用卡尔曼滤波算法完成对下述具体条件的目标位置估计。
具体条件为：目标从位置（100000m，2000m，0m）以速度（-200m/s，0 m/s，0 m/s）保持匀速飞行。初始位置、速度误差分别为100m（3σ），1m/s（3σ），雷达测量周期为1秒，测角误差为均方差0.
01（3σ）的白噪声，测距误差为均方差（3σ）的白噪声。
二、 算法模型。
1） 系统状态参数及状态方程。
系统的状态参数取为目标的位置和速度：，认为已知目标为匀速飞行，即。此时，状态空间方程为线性方程：
由上式可知，系统矩阵f为：
系统噪声u为：
2） 测量参数及测量方程。
测量参数取为雷达追踪的视距、方位角和高低角：
对方程（1.1）、（1.2）、（1.3）进行线性化处理：
其中。测量矩阵h为：
测量噪声为：
3） 状态和测量方程离散化。
1) 状态参数的状态转移矩阵。
首先求出转移矩阵的连续形式，可通过下式求得：
是单位阵，为拉普拉斯反变换，是前面得到的系统矩阵。此外，我们还能按照式（2.11）求出转移矩阵：
本系统，按照（2.11）求解方便，所以得到：
的离散形式就是将矩阵中的t换成采样步长ts：
可以看出，对于本问题，状态转移矩阵是常数矩阵。
2) 系统噪声协方差矩阵。
系统噪声矩阵q
将代入上式得。
其中，、、为加在目标加速度上的系统噪声的能量谱密度。系统噪声矩阵的离散化形式通过式（2.16）得到：
积分计算得：
同样，我们看出系统噪声矩阵在本问题中也是常值矩阵。
3) 测量矩阵离散化。
对于非线性卡尔曼滤波，每一次计算状态参数的估计值之前，需要先计算出参数的预估值（具体计算方法将在下面给出）。测量矩阵h的离散化形式就是将矩阵中的状态参数由预估值代替，具体形式如下：
4) 测量噪声协方差矩阵。
测量噪声矩阵r由式（2.19）求出：
假设三个测量噪声之间相互独立，将代入得：
其中，、、分别为三个测量噪声的方差。我们认为三个测量噪声为方差不变的白噪声，故其离散化形式为：
4） 状态参数迭代形式。
至此，我们已经求出了卡尔曼滤波所需的全部矩阵，下面我们推导出状态参数具体的迭代形式。
1) 状态参数的预估值。
我们知道，非线性卡尔曼滤波迭代的形式为：
其中，就是我们之前提到的预估值；是第k次迭代时的差值项，对于非线性卡尔曼滤波，，为测量值。我们对照标准线性卡尔曼滤波状态参数迭代方程：
括号内的项就相当于，而前面的项就相当于。对于本问题，由于状态方程为线性，与真实情况相匹配，为0，所以我们计算用下式：
代入前面求出的得：
2) 测量参数的预估值。
在上一小节我们提到，是测量值减去预估值，所以，我们还要利用前面状态参数的预估值求出测量参数的预估值：
其中，为已知的雷达位置。
3) 状态参数迭代方程。
其中，为第k次迭代卡尔曼增益矩阵（i,j）元素。各差值余项为：
4) 卡尔曼增益矩阵k迭代求解方程组。
卡尔曼增益矩阵k迭代求解方程组为：
其中，为每次迭代更新后状态参数误差（真实值减去估计值）协方差的理论值。为迭代前的理论值。启动求解方程组需要给出的初始值。
通常情况，我们取的对角元素为各状态参数初始误差的平方，非对角元素取为0。
三、 **计算。
1） **参数设置。
雷达位置：取为坐标原点。
测量噪声：测角噪声均方差；测距噪声均方差。测量噪声矩阵由式（2.21）求出。
导航原理大作业
导航原理作业 惯性导航部分 一 题目内容。一枚导弹采用捷联惯性导航系统，三个速率陀螺仪gx,gy,gz和三个加速度计 ax,ay,az的敏感轴分别沿着着弹体坐标系的xb,yb,zb轴。初始时刻该导弹处在北纬45.75度，东经126.63度。第一种情形 正对导弹进行地面静态测试 导弹质心相对地面静止 ...
北航惯性导航大作业
惯性导航基础课程大作业报告 一 光纤陀螺误差建模与分析。班级 111514 姓名 学号。2014年5月26日。一。系统误差原理图。二。系统误差的分析。一 漂移引起的系统误差。1.x，y，z对东向速度误差 vx的影响。clc clear all t 1 0.01 25 g 9.8 l pi 180 3...
北航惯性导航期末大作业
惯性导航期末大作业。14171106苗家语。1.平台式惯导系统，求导航坐标系n系与计算机坐标系c系的区别和转换矩阵，设c系相对n系的误差角为。答 n系与c系误差角为 三个欧拉角矩阵相乘，近似得到转换矩阵 2.以四元数相乘的法则计算两个矢量p和q的四元数相乘的结果。答 矢量p 矢量q p qp q p...