KalmanFiltering
说明：&&扩展卡尔曼滤波算法C语言实现：实现对二维状态变量的预测及跟踪。包括卡尔曼滤波初始化函数，负责初始化状态变量的值，过程噪声，测量噪声，以及状态转移矩阵。 卡尔曼滤波函数负责对状态变量进行迭代滤波，整个过程包括状态预测，协方差预测，测量预测，计算卡尔曼增益，状态量更新，协方差更新。(To realize the prediction and tracking of two dimensional state variables. It includes the initialization function of Kalman filter, which is responsible for initializing the value of state variables, process noise, measurement noise, and state transfer matrix. Kalman filter function is responsible for iterative filtering of state variables. The whole process includes state prediction, covariance prediction, measurement and prediction, computing Kalman gain, updating state variables and updating covariance.)
文件列表：
KalmanFilter.c, -11-21
KalmanFilter.h, 454,
近期下载者：
相关文件：扫二维码下载作业帮
3亿+用户的选择
下载作业帮安装包
扫二维码下载作业帮
3亿+用户的选择
卡尔曼滤波系统噪声和观测噪声怎么得到
作业帮用户
扫二维码下载作业帮
3亿+用户的选择
一般在算法研究中,都是假设系统噪声和观测噪声是已知的,根据不同的方差,利用编程语言中的随机函数获得.如,matlab中可以用random('norm',0,0.1);生成均值为0,方差为0.1的正态分布的噪声.
为您推荐：
其他类似问题
扫描下载二维码　　扩展卡尔曼滤波的状态方程和观测方程可以是非线性的。在一般情况下，无法确定过程噪声、测量噪声与方程的函数关系，因此可以简化为加性噪声：
　　EKF relies on a linearisation of the evolution and observation functions which are good&approximations of the original functions if these functions are close to linear. The state-space formulation of&EKF reads :&
　　Non-linear evolution and observation functions are&handled within EKF by linearising these functions around some es&for example for the evolution function is linearized around the previous estimate of the state $\hat{x}_k$:
$$f\left(x_k\right)\approx f\left(\hat{x}_k\right)+\frac{\partial f}{\text{dx}}\left(\hat{x}_k\right)\left(x_k-\hat{x}_k\right)$$
　　The first step in applying&EKF is to linearize the evolution function around the previous estimate of the state $\hat{x}_{k-1}$&
　　扩展卡尔曼滤波流程如下图所示：
&　　一个简单的例子：假设一架飞机以恒定水平速度飞行（高度不变），地面上有一个雷达可以发射电磁波测量飞机到雷达的距离$r$。则有如下关系：
$$\theta=arctan(\frac{y}{x})$$
$$r^2=x^2+y^2$$
　　我们想知道某一时刻飞机的水平位置和垂直高度，以水平位置、水平速度、垂直高度作为状态变量：
$$\mathbf{\textbf{x}}=\left(\begin{array}{c}\text{distance} \\\text{velocity} \\\text{altitude}\end{array}\right)=\left(\begin{array}{c}x \\\dot{x} \\y\end{array}\right)$$
　　则观测值与状态变量之间的关系为：$h\left(\hat{x}\right)=\sqrt{x^2+y^2}$，可以看出这是一个非线性的表达式。对于这个问题来说，观测方程的雅克比矩阵为：$J_H=\left[\frac{\partial h}{\partial x}\quad\frac{\partial h}{\partial \dot{x}}\quad\frac{\partial h}{\partial y}\text{ &}\right]$，即
　　$$J_H=\left[\frac{x}{\sqrt{x^2+y^2}} \quad 0 \quad \frac{y}{\sqrt{x^2+y^2}}\right]$$
　　状态转移方程的雅克比矩阵为：
　　得到上述矩阵后我们就可以设定初值和噪声，然后根据流程图中的步骤进行迭代计算。
MRPT中的卡尔曼滤波器
　　卡尔曼滤波算法都集中在 这个虚类中。 这个类中包括系统状态向量和系统协方差矩阵，以及根据选择的算法执行一个完整迭代的通用方法。在解决一个特定问题时需要从这个虚类派生一个新的类，并实现状态转移函数、观测函数以及它们的雅克比矩阵(采用EKF时)。内部的mrpt::bayes::CKalmanFilterCapable::runOneKalmanIteration()函数会依次调用用户改写的虚函数，每调用一次该函数执行一步预测+校正操作（runOneKalmanIteration():The main entry point, executes one complete step: prediction + update）
　　使用MRPT解决上述问题的C++代码如下：
#include &mrpt/bayes/CKalmanFilterCapable.h&
#include &mrpt/random.h&
#include &mrpt/system/os.h&
#include &mrpt/system/threads.h&
#include &iostream&
using namespace
using namespace mrpt::
using namespace mrpt::
using namespace mrpt::
using namespace mrpt::
using namespace
#define DELTA_TIME
// Time Step between Filter Steps
// 系统状态变量初始值(猜测值)
#define VEHICLE_INITIAL_X
#define VEHICLE_INITIAL_Y
#define VEHICLE_INITIAL_V
#define TRANSITION_MODEL_STD
// 模型噪声
#define RANGE_SENSOR_NOISE_STD
// 传感器噪声
/* --------------------------------------------------------------------------------------------
Virtual base for Kalman Filter (EKF,IEKF,UKF) implementations.
template&size_t VEH_SIZE, size_t OBS_SIZE, size_t FEAT_SIZE, size_t ACT_SIZE, typename KFTYPE&
class mrpt::bayes::CKalmanFilterCapable& VEH_SIZE, OBS_SIZE, FEAT_SIZE, ACT_SIZE, KFTYPE &
The meaning of the template parameters is:
VEH_SIZE: The dimension of the "vehicle state"(系统状态变量数目)
OBS_SIZE: The dimension of each observation (eg, 2 for pixel coordinates, 3 for 3D coordinates,etc).(观测量维数)
FEAT_SIZE: The dimension of the features in the system state (the "map"), or 0 if not applicable (the default if not implemented).
ACT_SIZE: The dimension of each "action" u_k (or 0 if not applicable).(控制量的维数)
KFTYPE: The numeric type of the matrices (default: double)
This base class stores the state vector and covariance matrix of the system. It has virtual methods
that must be completed by derived classes to address a given filtering problem.
---------------------------------------------------------------------------------------------- */
// Implementation of the system models as a EKF
class CRange: public CKalmanFilterCapable&<span style="color: #, <span style="color: #, <span style="color: #, <span style="color: #&
CRange( );
virtual ~CRange();
Process( double DeltaTime, double observationRange);
void getState( KFVector &xkk, KFMatrix &pkk)
//The system state vector.
//The system full covariance matrix
protected:
float m_obsR
float m_deltaT
// Time Step between Filter Steps
// return the action vector u
void OnGetAction( KFArray_ACT &out_u ) const;
// Implements the transition model
void OnTransitionModel(const KFArray_ACT &in_u,KFArray_VEH &inout_x,bool &out_skipPrediction) const;
// Implements the transition Jacobian
void OnTransitionJacobian(KFMatrix_VxV
&out_F ) const;
// Implements the transition noise covariance
void OnTransitionNoise(KFMatrix_VxV &out_Q ) const;
// Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
void OnGetObservationNoise(KFMatrix_OxO &out_R) const;
/** This is called between the KF prediction step and the update step
This method will be called just once for each complete KF iteration.
* \note It is assumed that the observations are independent, i.e. there are NO cross-covariances between them.
void OnGetObservationsAndDataAssociation(
vector_KFArray_OBS
mrpt::vector_int
&out_data_association,
const vector_KFArray_OBS
&in_all_predictions,
const KFMatrix
const vector_size_t
&in_lm_indices_in_S,
const KFMatrix_OxO
// Implements the observation prediction
void OnObservationModel(const vector_size_t &idx_landmarks_to_predict,vector_KFArray_OBS
&out_predictions) const;
// Implements the observation Jacobians
void OnObservationJacobians(const size_t &idx_landmark_to_predict,KFMatrix_OxV &Hx,KFMatrix_OxF &Hy) const;
CRange::CRange()
KF_options.method = kfEKFN
// 状态变量初始值 State: (x,vx,y)
m_xkk.resize(<span style="color: #);
//对于动态矩阵可以通过resize()函数来动态修改矩阵的大小
m_xkk[<span style="color: #]= VEHICLE_INITIAL_X;
m_xkk[<span style="color: #]= VEHICLE_INITIAL_V;
m_xkk[<span style="color: #]= VEHICLE_INITIAL_Y;
// Initial cov:
Large uncertainty
m_pkk.setSize(<span style="color: #,<span style="color: #);
m_pkk.unit();
m_pkk = <span style="color: # * m_
CRange::~CRange()
CRange::Process( double DeltaTime, double observationRange)
m_deltaTime = (float)DeltaT
m_obsRange
= (float)observationR
runOneKalmanIteration(); // executes one complete step: prediction + update
// Must return the action vector u.
// param out_u: The action vector which will be passed to OnTransitionModel
void CRange::OnGetAction( KFArray_ACT &out_u ) const
/** Implements the transition model(Project the state ahead)
param in_u :
The vector returned by OnGetAction.
param inout_x:
prediction value
param out_skip: Set this to true if for some reason you want to skip the prediction step. Default:false
void CRange::OnTransitionModel(const KFArray_ACT &in_u, KFArray_VEH &inout_x, bool &out_skipPrediction) const
// The constant-velocities model is implemented simply as:
inout_x[<span style="color: #] +=
m_deltaTime * inout_x[<span style="color: #];
inout_x[<span style="color: #] = inout_x[<span style="color: #];
inout_x[<span style="color: #] = inout_x[<span style="color: #];
/** Implements the transition Jacobian
param out_F Must return the Jacobian.
The returned matrix must be N*N with N being the size of the whole state vector.
void CRange::OnTransitionJacobian(KFMatrix_VxV
F(<span style="color: #,<span style="color: #) = m_deltaT
/** Implements the transition noise covariance
param out_Q Must return the covariance matrix.
The returned matrix must be of the same size than the jacobian from OnTransitionJacobian
void CRange::OnTransitionNoise(KFMatrix_VxV &Q) const
Q *= square(TRANSITION_MODEL_STD);
/** Return the observation NOISE covariance matrix, that is, the model of the Gaussian additive noise of the sensor.
param out_R : The noise covariance matrix. It might be non diagonal, but it'll usually be.
void CRange::OnGetObservationNoise(KFMatrix_OxO &R) const
R *= square(RANGE_SENSOR_NOISE_STD);
// This is called between the KF prediction step and the update step
void CRange::OnGetObservationsAndDataAssociation(
vector_KFArray_OBS
mrpt::vector_int
&out_data_association,
const vector_KFArray_OBS
&in_all_predictions,
const KFMatrix
const vector_size_t
&in_lm_indices_in_S,
const KFMatrix_OxO
//out_z: N vectors, N being the number of "observations"
out_z.resize(<span style="color: #);
out_z[<span style="color: #][<span style="color: #] = m_obsR
/** Implements the observation prediction
param idx_landmark_to_predict: The indices of the landmarks in the map whose predictions are expected as output. For non SLAM-like problems, this input value is undefined and the application should just generate one observation for the given problem.
param out_predictions: The predicted observations.
void CRange::OnObservationModel(const vector_size_t &idx_landmarks_to_predict,vector_KFArray_OBS &out_predictions) const
// idx_landmarks_to_predict is ignored in NON-SLAM problems
out_predictions.resize(<span style="color: #);
out_predictions[<span style="color: #][<span style="color: #] = sqrt( square(m_xkk[<span style="color: #]) + square(m_xkk[<span style="color: #]) );
// Implements the observation Jacobians
void CRange::OnObservationJacobians(const size_t &idx_landmark_to_predict,KFMatrix_OxV &Hx,KFMatrix_OxF &Hy) const
Hx.zeros();
Hx(<span style="color: #,<span style="color: #) = m_xkk[<span style="color: #] / sqrt(square(m_xkk[<span style="color: #])+square(m_xkk[<span style="color: #]));
Hx(<span style="color: #,<span style="color: #) = m_xkk[<span style="color: #] / sqrt(square(m_xkk[<span style="color: #])+square(m_xkk[<span style="color: #]));
int main ()
// Create class instance
EKF.KF_options.method = kfEKFN //select the KF algorithm
// Initiate simulation
float x=<span style="color: #, y=<span style="color: #00, v=<span style="color: #0; //状态变量真实值
float t=<span style="color: #;
while (!mrpt::system::os::kbhit())
// Simulate noisy observation:
x += v * DELTA_TIME;
float realRange = sqrt(square(x)+square(y));
// double mrpt::random::CRandomGenerator::drawGaussian1D_normalized(double * likelihood = NULL)
// Generate a normalized (mean=0, std=1) normally distributed sample
float obsRange = max(<span style="color: #.0, realRange + RANGE_SENSOR_NOISE_STD * randomGenerator.drawGaussian1D_normalized() );
printf("Real/Simulated range: %.03f / %.03f \n", realRange, obsRange );
// Process with EKF
EKF.Process(DELTA_TIME, obsRange);
// Show EKF state:
CRange::KFVector EKF_
CRange::KFMatrix EKF_
EKF.getState( EKF_xkk, EKF_pkk );
printf("Real state: x:%.03f
y=%.03f \n",x,v,y);
cout && "EKF estimation:" &&endl&& EKF_xkk &&
cout &&"-------------------------------------------"&&
// Delay(An OS-independent method for sending the current thread to "sleep" for a given period of time)
mrpt::system::sleep((int)(DELTA_TIME*<span style="color: #00));
t += DELTA_TIME;
return <span style="color: #;
　　运行一段时间后结果如下图所示，可以看出状态变量基本收敛到真实值（由于传感器和模型噪声不可消除，因此只能是对真实状态的最优估计）。
阅读(...) 评论()华 北 电 力 大 学 毕业设计（论文）文献综述
电力工程系
指导教师签名
审批人签字
毕业设计(论文)题目 基于卡尔曼滤波原理的电网频率综合检测 和预测方法的研究
基于卡尔曼滤波原理的电网频率综合检测和预测方法的研究 一、前言
“频率”概念源于针对周期性变化的事物的经典物理学定义，由于电力系统中许多物理变量具有（准）周期性特征，故这一概念得到广泛应用【1】。 电网频率是电力系统运行的主要指标之一，也是检测电力系统工作状态的重要依据，频率质量直接影响着电力系统安全、优质、稳定运行。因此，频率检测和预测在电网建设中起着至关重要的作用。 随着大容量、超高压、分布式电力网网络的形成以及现代电力电子设备的应用，基于传统概念的电力系统频率和测量技术在解决现代电网频率问题上遇到了诸多挑战。 目前，用于频率检测和预测的方法很多，主要有傅里叶变换法、卡尔曼滤波法、最小均方误差法、正交滤波器法、小波变换法、自适应陷波滤波器以及它们和一些算法相结合来解决电网频率检测和预测问题。 本文着重讲述卡尔曼滤波原理、分类以及它在电力系统频率检测中的应用历程进行系统性分析，并对今后的研究方向做出展望。 二、主题 1 常规卡尔曼滤波 常规卡尔曼滤波是卡尔曼等人为了克服维纳滤波的不足，于60年代初提出的一种递推算法。卡尔曼滤波不要求保留用过的观测数据，当测得新的数据后，可按照一套递推公式算出新的估计量，不必重新计算【2】。 下面对其进行简单介绍： 假设线性离散方程为 xk?1?Akxk??k（1） zk?Hkxk??k （2） 式子中：xk ?Rn为状态向量；zk?Rm为测量向量；?k?Rp为系统噪声或过程噪声向量；?k?R为量测噪声向量；Ak为状态转移矩阵；Hk为量测转移转移矩阵。假设系统噪声和量测噪声是互不相关的高斯白噪声，方差阵为Qk、Rk，定义xk/k?1=E(xk|yk?1) 其他递推，则卡尔曼滤波递推方程如下： 状态1步预测为
???mxk/k?1=Akxk?1（3） 1步预测误差方差阵为 TPk/k?1=Ak?1Pk?1Ak?1+Qk?1（4） 状态估计为 xk=xk/k?1+Kk（zk-Hkxk/k?1）（5） ???估计误差方差阵为 Pk=（I-KkHk）Pk/k?1（6） 滤波增益矩阵为 ?1TT（HkPk/k?1Hk+Rk）（7） Kk=Pk/k?1Hk式中I为单位阵。式（3）~（7）就是随机离散系统卡尔曼滤波的基本方程【3】。 与常见的FFT、DFT比较，卡尔曼滤波不会出现采不到高频谐波或者泄露的情况。 2 扩展卡尔曼滤波 由于实际系统存在非线性因素，使得传统的卡尔曼滤波在频率检测预测方面存在困难，于是便有了诸多针对非线性模型的次优方法即扩展卡尔曼滤波（extended Kalman filtering,EKF）。EKF是将非线性模型线性化，它的主要思想是对非线性函数的泰勒级数展开式进行截断，实现线性化。 与线性卡尔曼滤波相似其原理如下： xk?1?F(xk,?k,k)（8） zk?H(xk,k)??k（9） 式（8）（9）分别是状态方程和量测方程。扩展卡尔曼滤波必须在指定位置进行泰勒级数展开，实现线性化。过程如下： xk?F(xk?1/k?1)??F(xk?1)|xk?1/?1??k?xk?1（10） ?H(xk)|xk/k?1??k?xk（11） yk?H(xk/?1)?1991年，Beides H M和Heydet G T提出用扩展卡尔曼滤波理论来动态估计电力系统谐波。1993年，Kamwa也将EKF引入电力系统电能质量分析中，用于测量闪变。文献[4]根据离散的电网三相电压信号，在存在系统噪声和信号严重畸变的情况下，利用扩展卡尔曼滤波实现频率的正确估计。文献[5]提出一种以自适应卡尔曼滤波为基础的动态估计算法，提高滤波精度减小误差。但是当系统负荷突然变大时该算法误差较大。文献[6]通过分析单纯采用卡尔曼滤波算法误差较大和采用奇异值分解算法的动态估计算法具有好的收敛性和精度，提出了两者相结合的算法，即在算法程序初始时刻和不满足准稳态的前提时启用SVD算法，为卡尔曼滤波算法提供基准状态数据和向管理者提供谐波数据库。 3 无迹卡尔曼滤波 在扩展卡尔曼滤波中，系统状态分布和所有的相关噪声密度由高斯随机变量近似，其均值和方差解析地通过一个非线性系统的一阶线性化传播。这样会给变换后的高斯随机变量的真实验后均值和方差带来较大的误差，从而导致次优解甚至使滤波发散。Sigma点卡尔曼滤波利用一个确定性采样方式来解决这个问题。状态分布同样用高斯随机变量来近似，但是用一个精心挑选的加权采样点的最小集来表示。利用这些点可以较好的描述高斯随机变量的真实均值和方差，并且当通过真实的非线性系统传播时，获得的验后均值和方差的精度为非线性系统的二阶泰勒级数展开的结果，然而扩展卡尔曼滤波仅能达到一阶泰勒级数展开的精度。更重要的是Sigma点卡尔曼滤波实现起来比扩展卡尔曼滤波简单，不需要计算雅克比矩阵。最为熟知的是基于无迹变换的无迹卡尔曼滤波，此外还有平方根无迹卡尔曼滤波、中心微分变换的中心微分卡尔曼滤波和平方根中心微分卡尔曼滤波等等。 为了改善对非线性问题进行滤波的效果，Julier等提出了基于无迹变换（简称U变换）的无迹卡尔曼滤波方法。该方法在处理状态方程时候，首先进行U变换，然后利用变换后的状态变量进行滤波估计，以减少估计误差。 Sigma点的创建方式： 设n维随机变量X~N（x，PX），m维随机向量Z为X的某一非线性函数Z=f(X)通过非线性函数f(?)进行传播得到Z的统计特性（Z，PZ）。U变换就是根据（x，PX）设计一些列点?i（i=1,2,3,?，L）称其为Sigma点。将其经过f(?)传播得到的结果?i（i=1,2,3,?，L）；然后基于?i计算（Z，PZ）。 U变换的具体过程如下： （1）计算2n+1个Sigma点及其权系数 _??_?0?X（12） _?i?X?((n??)PX)i,i?1,2,???,n（13） _?i?X?((n??)PX)i,i?n?1,n?2,???,2n（14） ?0(m)?(c)?0?_?n??
（15） ?n???(1??2??)（16） ?i(m)??i(c)??2(n??),i?1,2,???,2n（17） ???2(n?k)?n（18） 式中：系数?决定Sigma点的散布程度，通常取一个很小的数值如0.01；k通常取0；?用来描述X的分布信息，通常高斯噪声情况取值为2；((n??)PX)i表示矩阵平方根的第i列；?i(m)为求一阶统计特性的权系数；?i(c)为二阶统计特性时的权系数。 （2）计算Sigma点经过非线性函数f(?)的传播结果 ?i=f(?i),i=0,1,2，?，2n（19） 从而 PZ???(??Z)(?i?Z)T（21） PXZ???(?i?Z)(?i?Z)T（22） (c)i2ni?0__2nZ???i(m)?i（20） (c)i?0ii___2n下面介绍无迹卡尔曼滤波的具体过程： 考虑如下非线性模型 i?0Xk?1?F(Xk,uk,?k)（23） Zk?H(Xk,?k)（24） 式中定义与扩展卡尔曼滤波相似。则无迹卡尔曼滤波计算过程如下： （1）初始化 X0?E[X0],PX0?E[(X0?X0)(X0?X0)T](25) ^^^X0?E[X]?[X,?,?](26) P?E[(X?X0)(X?X0)T](27) (2)对于给定的Xk?1，P，用U变换求状态一步预测Xk/k?1和Pka/k?1，执行步骤如下： 1计算Sigma点?○(i)k?1（^a^aa0^T0_k0_TT0a0a0^aa0^aak?1^ai?1,2,???,n）， ?(0)k?1?Xk?1（28） ^a?(i)k?1?Xk?1?((n??)Pka?1)i,i?1,2,???,n（29） ^a?(i)k?1?Xk?1?((n??)Pka?1)i,i?n?1,n?2,???,2n（30） ^a2计算时间更新方程。通过状态方程的传播计算Sigma点?k(i)（i?0,1,2,???,2n）， ○,10,2,，i?2?k(i)=F（?k(i?)1）^a???n（31） Xk/k?1=??i(m)?k(i)（32） i?02nPak/k?1=??i?02n(c)i(?i?Xk/k?1)(?i?Xk/k?1)T?Pka?1（33） ^a^a3计算观测更新方程 ○?k/k?1?H(?ka/k?1)（34）