容积卡尔曼滤波（ckf）MATLAB代码(一个ckf滤波器的子程序)

一个ckf滤波器的子程序，自己可以进行拓展，可以根据自己的实例进行套用，适用与多维实例的系统，代码注释全面，参数设置参考的文献均已注明，值得学习

%% ---------------------------------------------------------------
% Version: 20121120, one of the functions for Nonlinear Filtering.All
% Rights Reserved. Anyone can use it for your own simulations. Do not modify
% the following comments.
%
% Implementation of the third-degree cubature Kalman filters. This script
% could be applied to multi-dimensional models.
%
% Caution: For validating the third-degree cubture Kalman fiters only,
% the model employed is known as the bearing-only tracking (four-dimensional
% model), the values of parameters can be referred to the following
% references:
% [1] I. Arasaratnam, S. Haykin, "Cubature Kalman filters," IEEE Trans.
% Automat. Control, vol. 54, no. 6, 2009, pp. 1254-1269.
% [2] X. C. Zhang, C. J. Guo, "Square-root imbedded cubature Kalman
% filtering," Control Theory & Applications, to appear. (In Chinese).
% [3] X. C. Zhang, C. J. Guo, "Cubature Kalman filters: Derivation and
% extension," Chinese Physics B, accepted.
%
%
%                                              - By Irvingzhang(UESTC)
%                                                    2012-11-20
%                                      Email Address: irving_zhang@163.com
%% ---------------------------------------------------------------
function [x, P] = CKF(xhat, Pplus, z)
global Q R fai gama kesi w m;
%% -----------------------------Time Update-----------------------% Evaluate the Cholesky factorShat = sqrtm(Pplus);for cpoint = 1 : m% Evaluate the cubature pointsrjpoint(:, cpoint) = Shat * kesi(:, cpoint) + xhat;% Evaluate the propagated cubature pointsXminus(:, cpoint) = fai * rjpoint(:, cpoint); end% Estimate the predicted statexminus = w * sum(Xminus, 2);% Estimate the predicted error covariancePminus = w * (Xminus * Xminus') - xminus * xminus' + gama * Q * gama';
%% ---------------------------------------------------------------%% -------------------------Measurement Update--------------------% Evaluate the Cholesky factorSminus = sqrtm(Pminus);for cpoint = 1 : m% Evaluate the cubature pointsrjpoint1(:, cpoint) = Sminus * kesi(:, cpoint) + xminus;% Evaluate the propagated cubature pointsZ(:, cpoint)=atan(rjpoint1(3, cpoint) / rjpoint1(1, cpoint));end    % Estimate the predicted measurementzhat = w * sum(Z, 2);% Estimate the innovation covariance matrixPzminus = w * Z * Z'-zhat * zhat' + R;% Estimate the cross-covariance matrixPxzminus = w * rjpoint1 * Z'- xminus * zhat';% Estimate the Kalman gainW = Pxzminus * inv(Pzminus);% Estimate the updated statex = xminus + W * (z - zhat);% Estimate the correspondong error covarianceP = Pminus - W * Pzminus * W';

容积卡尔曼滤波（ckf）MATLAB代码(一个ckf滤波器的子程序)相关推荐

读懂matlab代码,一个Matlab的寻峰程序没有看懂，不知大家能否帮助？
这个程序用来寻找输入的两组数据(x,y)的峰,并计算其半高全宽和其他参数. 开始比较容易懂,后来到了程序主体部分,看不明白,看了好长时间,可能因为知识结构不全面,看不懂了.希望再此能得到大家的帮助,多 ...
从贝叶斯滤波理论到容积卡尔曼滤波算法（CKF）详细推导及编程实现常转弯率模型估计。（matlab）
容积卡尔曼滤波(CKF)是由加拿大学者Arasaratnam和Haykin在2009年提出的.该算法的核心思想是针对非线性高斯系统,通过三阶球面径向容积准则来近似状态的后验均值和协方差,以保证在理论上 ...
卡尔曼滤波MATLAB代码实现
没有大量的公式推导,个人感觉也没有必要,我们从小推导过很多公式,试着想想我们还能回忆起几个?个人认为只需要记住公式的用法,作用,知道有这个公式就可以.用的时候我们可以随时去查.所以楼主参考网上资料结合 ...
matlab函数无限冲激响应滤波器,MATLAB代码有限冲激响应（FIR）滤波器和无限冲激响应（IIR）滤波器...
MATLAB有限冲激响应(FIR)滤波器和无限冲激响应(IIR)滤波器设计附MATLAB代码摘要文章设计了一个数字信号处理仿真实验,产生一个信号,其频率成分为f1和f2,并对其进行理想采样,采样 ...
基于扩展卡尔曼滤波的SOC估计（附MATLAB代码）
1.卡尔曼滤波原理原理可以参考我之前学习的笔记,使用goodnote完成的. 我认为,对于公式的推导不需要做太多深入的了解,我之前也对公式进行推导的理解,但是没过几天就忘了,只需要掌握住那重要的5个 ...
KF、EKF、UKF的matlab代码实现
概述本文分享卡尔曼滤波.扩展卡尔曼滤波和无迹卡尔曼滤波的matlab代码实现. 正文卡尔曼滤波: %{X(k) = F*X(k-1) + Q %预测方程 Y(k) = H*X(k) + R %观测 ...
无迹卡尔曼滤波UKF和容积卡尔曼滤波CKF进行锂电池SOC估计的C语言版本实现，和matlab版本一样包含定参和FFRLS两种情况
(C语言版)无迹卡尔曼滤波UKF和容积卡尔曼滤波CKF进行锂电池SOC估计的C语言版本实现,和matlab版本一样包含定参和FFRLS两种情况,已在VS2019和Ubuntu 20.04.4版本中运行 ...
基于容积卡尔曼滤波(CubatureKalmam Filter, CKF)的车辆状态观测器 Carsim与Simulink联合可生成C代码
基于容积卡尔曼滤波(CubatureKalmam Filter, CKF)的车辆状态观测器 Carsim与Simulink联合可生成C代码 ?CKF算法使用子函数形式编程,在定义好状态方程和观测方程 ...
容积卡尔曼滤波CKF—目标跟踪中的应用(算法部分—I)
容积卡尔曼滤波CKF-目标跟踪中的应用(算法部分) 原创不易,路过的各位大佬请点个赞机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码联系WX: ZB823618313 作者:823618313@ ...

容积卡尔曼滤波（ckf）MATLAB代码(一个ckf滤波器的子程序)

容积卡尔曼滤波（ckf）MATLAB代码(一个ckf滤波器的子程序)相关推荐

最新文章

热门文章