搜索
王文科2018/11/01矩阵运算简介矩阵加减：同型矩阵才能进行加减运算，运算时各个对应元素相加减，运算满足结合律、交换律。矩阵相乘：例如C=AB，C的行数与A行数相同，C的列数与B列数相同，C的第i行j列的元素由A的第i行与B的第j列对应相乘。如A是mxn矩阵B是nxp矩阵，那么C是mxp矩阵。不满足交换律。矩阵转置：将矩阵A的行换成同序号的列得到的新矩阵称为A的转置矩阵。单位矩阵：如同数的乘法中的1。它是个方阵，从左上角到右下角的对角线（称为主对角线）上的元素均为1。除此以外全都为0。卡尔曼滤波更新公式中的I即指单位矩阵。两矩阵相加减：两矩阵相乘：矩阵转置：方差、标准差与协方差方差：方差用来计算每一个变量（观察值）与总体均数之间的差异。为避免出现离均差总和为零，离均差平方和受样本含量的影响，统计学采用平均离均差平方和来描述变量的变异程度。标准差：对方差开平方。协方差：在概率论和统计学中用于衡量两个变量的总体误差。而方差是协方差的一种特殊情况，即当两个变量是相同的情况。如果两个变量的变化趋势一致，也就是说如果其中一个大于自身的期望值，另外一个也大于自身的期望值，那么两个变量之间的协方差就是正值。反之，两个变量之间的协方差就是负值。方差的两个公式：协方差公式：正态分布与标准差：方差为协方差的特殊情况：注：使用部分样本统计被测对象的方差时，为了达到无偏估计，使用(n-1)作为分母。协方差矩阵协方差矩阵：协方差矩阵的每个元素是各个向量元素之间的协方差，是从标量随机变量到高维度随机向量的自然推广。卡尔曼滤波器简介卡尔曼滤波器是一种高效率的递归滤波器，它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。得名自主要贡献者之一的鲁道夫·卡尔曼（匈牙利裔美国数学家）。对于一个线性系统，卡尔曼滤波器能够从不精确的预测状态和观测状态中，估算出高精度的系统状态，并且估计过程只需要保留最近一次的估算结果，具有速度快、资源需求低的特点。其滤波过程为：根据当前状态和系统方程估算下一状态获取下一状态的观测结果使用当前卡尔曼增益加权平均更新估计值更新卡尔曼增益。整个过程迭代执行。更新估计值时预测值和观测值所占权重由其不确定性决定，基本卡尔曼滤波器擅长处理正态分布的误差。卡尔曼滤波包含噪声的预测状态包含噪声的观测状态接近真实状态的结果卡尔曼滤波器公式介绍卡尔曼滤波器有5个基本公式，其中2个为预测公式，其余3个为更新公式。F：状态转移矩阵；B：控制矩阵；P：表示系统不确定性的协方差矩阵；Q：表示预测值的协方差矩阵；R：表示测量值的协方差矩阵；H：系统状态到观测状态的变换矩阵；K：卡尔曼增益；P会快速迭代，初始值选取对滤波效果影响很小；Q一般是对角阵，且对角线上的值很小，较难确定；R是一个数值，是和仪器相关的一个特性，作为已知条件输入滤波器。预测公式更新公式^表示该值为估计值-表示该状态根据上一状态推测····状态预测公式····不确定性转移公式实际观察值与预估的观测值之间的残差应用举例-室内温度估算卡尔曼滤波器运用的一个简单例子是用于测试一个房间的温度值，假设房间温度在观测过程中是恒定的，同时每过单位时间用温度计测量房间温度，预测和测量结果都存在误差，假设其为正态分布。在t-1时刻的最优值为23℃，该温度的偏差为3℃；t时刻的预测偏差为4℃，t时刻温度计测得温度25℃，其偏差为4℃。求解t时刻房间温度的最优值。根据上一时刻温度预测该时刻温度不变为23℃该问题下协方差与方差相同，由标准偏差求得方差计算卡尔曼系数计算出t时刻温度最优值为24.22℃更新t时刻温度最优值方差为9.75，其标准差为3.12℃该问题状态只有温度一个量，且预测温度不变，卡尔曼滤波公式简化为：求解过程：应用举例-小车状态估算有一做匀速直线运动的小车，状态包含位置p和速度v，每经过时间Δt使用测距仪测量小车位置。使用卡尔曼滤波器预测车辆的位置和速度，驾驶初始状态时小车的位置和速度向量不具有相关性，p和v的协方差为0；该系统存在预测协方差矩阵Q、测量协方差矩阵R、系统协方差矩阵P（持续迭代）；可以得出F、P、Q、H、R的基本形式如图所示；小车为匀速运动，不存在控制矩阵，公式简化为：求解过程：应用举例-小车状态估算Matlab模拟小车位置设定为1～200，时间步长为1，状态初始值给[0;0]，位置观测值叠加方差为1的高斯噪声；给出假定的预测协方差矩阵Q、观测噪声方差R；滤波结果如图所示，滤波值很快收敛到真实速度1附近。滤波结果卡尔曼滤波器参数特性P的初始参数对卡尔曼滤波效果影响不大，但P0/(Q+R)会影响滤波结果的收敛速度；如图，分别使用[10;01]和[22;22]作为P的初始值，经过30次迭代，滤波结果就已经基本相同；两种初始P值经过迭代后，最终均为[0.13220.0093;0.00930.0014]。卡尔曼滤波器参数特性Q/(Q+R)的值代表卡尔曼增益的收敛值，卡尔曼增益越大，说明测量值越可靠，最优化结果越接近测量值；卡尔曼增益越小，说明预测值越可靠，最优化结果越接近测量值；如图，较大的Q参数表示预测值可信度低，滤波结果最终接近观测值（观测值加入了偏差为1的高斯噪声，误差较大）；卡尔曼滤波器参数特性R参数是测量值的协方差矩阵，用于表示测量数据的误差，单一测量结果的R参数是一个数值，该值的大小由测量设备本身决定；R值的大小会影响卡尔曼滤波的收敛速度和最终滤波精度；