做网站怎么接单,wordpress博客 文章格式,网站title 在哪里设置,百姓网招聘信息目录 引言Kalman滤波代码及其结果展示 扩展Kalman滤波代码及其结果展示 无迹Kalman滤波无迹变换无迹Kalman滤波代码及其结果展示 异步无迹Kalman滤波原理代码及其结果展示 引言
本文给出了Kalman Filter#xff08;卡尔曼滤波#xff09;、Extended Kalman Filter#xff0… 目录 引言Kalman滤波代码及其结果展示 扩展Kalman滤波代码及其结果展示 无迹Kalman滤波无迹变换无迹Kalman滤波代码及其结果展示 异步无迹Kalman滤波原理代码及其结果展示 引言
本文给出了Kalman Filter卡尔曼滤波、Extended Kalman Filter扩展卡尔曼滤波、Unscented Kalman Filter无迹卡尔曼滤波和Asynchronous Filter异步滤波的原理及其Matlab代码。不同的滤波算法以函数形式在不同的m文件调用即可使用。
Kalman滤波
该部分展示了kalman滤波的代码及其仿真结果。 状态转移模型 x ( t 1 ) A x ( t ) Γ w ( t ) x(t1)Ax(t)Γw(t) x(t1)Ax(t)Γw(t) 其中 x ( t ) ∈ R n ∗ 1 x(t)∈R^{n*1} x(t)∈Rn∗1表示t时刻的状态向量 A ∈ R m ∗ m A∈R^{m*m} A∈Rm∗m是状态转移矩阵,Γ是噪声系数矩阵w(t)是t时刻的过程噪声向量, 其为零均值高斯白噪声协方差阵为Q0。 量测模型 y i ( t ) H i x ( t ) v i ( t ) yi(t)Hi x(t)vi(t) yi(t)Hix(t)vi(t) ,其中 y i ( t ) yi(t) yi(t)表示t时刻传感器网络中的传感器i的量测向量 H i ∈ R l ∗ m Hi∈R^{l*m} Hi∈Rl∗m是观测矩阵,vi(t)是t时刻的量测噪声向量其为零均值高斯白噪声协方差阵为Ri0。 初始化: 状态更新:
代码及其结果展示
核心代码完整代码见文末
function [estimate, cov_error_estimate] kalman_filter(measure, x_last,...P_last,flag)
%kalman_filter 基本kalman滤波算法
% 输入量测值上一时刻估计上一时刻估计误差协方差,是否预测的标志
% 输出当前时刻估计当前时刻估计误差协方差
% 当flag为1则跳过预测步骤
%%
% 状态转移矩阵
global A;
% 系统噪声系数矩阵
global gama;
% 系统噪声协方差
global Q;
% 量测矩阵
global H;
% 量测噪声协方差
global R;
%%
% 如果全零即无初始化
if ~any(x_last)if measure0estimate x_last;cov_error_estimate P_last;else% 如果是0时刻则取量测值对应的状态为当前时刻估计9倍的量测误%差协方差阵为当前时刻估计误差协方差。estimate H\measure;cov_error_estimate H\(2*(R))/H;end
elseif flag1forecastx_last;P_forecastP_last;elseforecast A * x_last;P_forecast (A*P_last *A gama*Q* gama);endif measure0estimateforecast;cov_error_estimate P_forecast;elsekalman_gain P_forecast*H/(H*P_forecast*HR);estimate forecast kalman_gain*(measure - H*forecast);cov_error_estimate P_forecast - kalman_gain*H*P_forecast;end
end
end结果展示
扩展Kalman滤波
扩展卡尔曼滤波是标准卡尔曼滤波在非线性情形下的一种扩展形式EKF算法是将非线性函数进行泰勒展开省略高阶项保留展开项的一阶项以此来实现非线性函数线性化最后通过卡尔曼滤波算法近似计算系统的状态估计值和方差估计值,对信号进行滤波。 状态转移方程与量测方程 泰勒线性化 预测步骤 估计步骤
代码及其结果展示
核心代码完整代码见文末
function [estimate, cov_error_estimate] extend_kf(measure, x_last, P_last,...flag,radar)
%kalman_filter 扩展kalman滤波算法
% 输入量测值上一时刻估计上一时刻估计误差协方差,是否预测的标志,
%雷达的状态信息
% 输出当前时刻估计当前时刻估计误差协方差
% 当flag为1则跳过预测步骤
%%
% 状态转移矩阵
global A;
% 系统噪声系数矩阵
global gama;
% 系统噪声协方差
global Q;
% 量测噪声协方差
global R;
%%
% 如果全零即无初始化
if ~any(x_last)if measure0estimate x_last;cov_error_estimate P_last;else% 如果是0时刻则取量测值对应的状态为当前时刻估计9倍的量测误%差协方差阵为当前时刻估计误差协方差。% 此处为9的原因是3倍的标准差为百分之95的可能的误差值可以认为%包含了最大误差的情况。estimate[radar(1,1)measure(1,1)*cos(measure(3,1))*cos(measure(2,1));radar(2,1)measure(1,1)*sin(measure(3,1));radar(3,1)-measure(1,1)*cos(measure(3,1))*sin(measure(2,1));0;0;0];cov_error_estimate 99*[1,0,0,1,0,0;0,1,0,0,1,0;0,0,1,0,0,1;1,0,0,1,0,0;0,1,0,0,1,0;0,0,1,0,0,1];end
elseif flag1forecastx_last;P_forecastP_last;elseforecast A * x_last;P_forecast (A*P_last *A gama*Q* gama);endxforecast(1,1)-radar(1,1);yforecast(2,1)-radar(2,1);zforecast(3,1)-radar(3,1);dist norm([x;y;z]);dist_horiz norm([x;z]);H[x/dist,y/dist,z/dist,0,0,0;z/dist_horiz^2,0,-x/dist_horiz^2,0,0,0;-x*y/(dist^2*dist_horiz),dist_horiz/dist^2,-y*z/(dist_horiz*dist^2),0,0,0];z_est measure_func([x;y;z]);if measure0estimateforecast;cov_error_estimate P_forecast;elsekalman_gain P_forecast*H/(H*P_forecast*HR);estimate forecast kalman_gain*(measure - z_est);cov_error_estimate P_forecast - kalman_gain*H*P_forecast;end
end
end结果展示
无迹Kalman滤波
无迹变换
设存在一个m维随机向量和n维随机向量z其中z与x为非线性关系 x的均值为方差为Px通过非线性映射得到z。无迹变换就是根据x的统计特性得到一族点集该点集称为为σ点将σ点进行非线性映射可以得的新的点集通过新的点集得到z的统计特性。一般情况下σ点的数目为2n1。 无迹变换的具体过程可描述如下 (1)计算2n1个点及其权系数 其中决定点的散布程度通常取正数通常取为0β用来代表x的分布情况(正态分布的情况下最佳值为2)表示矩阵的平方根的第i列为用于计算均值的权重为用于计算方差时的权重。 (2)计算σ点通过非线性函数的传播结果 从而可得 此时我们得到了无迹变换的结果。
无迹Kalman滤波
针对量测为非线性、状态转移为线性的系统模型无迹Kalman滤波是用无迹变换进行经典卡尔曼滤波中量测方程的映射。 每个递推无迹Kalman滤波的具体步骤如下 (1)针对线性目标状态转移模型对于给定的估计和估计误差协方差基于经典Kalman滤波求状态预测以及预报误差的协方差阵。
(2)针对非线性传感器量测模型用无迹变换求σ点并通过量测方程的传播。 计算σ点即 计算输出的一步提前预测即 在获得新的量测后进行滤波更新
代码及其结果展示
核心代码完整代码见文末
function [estimate, cov_error_estimate] unscented_kf(measure, x_last, P_last,...flag,radar)
%kalman_filter 扩展kalman滤波算法
% 输入量测值上一时刻估计上一时刻估计误差协方差,是否预测的标志,
%雷达的状态信息
% 输出当前时刻估计当前时刻估计误差协方差
% 当flag为1则跳过预测步骤
%%
% 状态转移矩阵
global A;
% 系统噪声系数矩阵
global gama;
% 系统噪声协方差
global Q;
% 量测噪声协方差
global R;
%%
% 如果全零即无初始化
if ~any(x_last)if measure0estimate x_last;cov_error_estimate P_last;else% 如果是0时刻则取量测值对应的状态为当前时刻估计9倍的量测误%差协方差阵为当前时刻估计误差协方差。% 此处为9的原因是3倍的标准差为百分之95的可能的误差值可以认为%包含了最大误差的情况。% TODO 应该设置为根据实际对象进行变化这里强制设置为6个状态%后续对无迹滤波和此处进行改变让其可以根据实际情况进行随动estimate[radar(1,1)measure(1,1)*cos(measure(3,1))*cos(measure(2,1));radar(2,1)measure(1,1)*sin(measure(3,1));radar(3,1)-measure(1,1)*cos(measure(3,1))*sin(measure(2,1));0;0;0];cov_error_estimate 1*[1,0,0,1,0,0;0,1,0,0,1,0;0,0,1,0,0,1;1,0,0,1,0,0;0,1,0,0,1,0;0,0,1,0,0,1];end
elseif flag1forecastx_last;P_forecastP_last;elseforecast A * x_last;P_forecast (A*P_last *A gama*Q* gama);endif measure0estimateforecast;cov_error_estimate P_forecast;else[forecast_point,forecast_weight]get_point(forecast,P_forecast,size(forecast,1));measure_point zeros(size(measure,1),size(forecast_point,2));for cou_point 1:size(forecast_point,2)measure_point(:,cou_point)measure_func(forecast_point(1:3,cou_point)-radar(1:3,1));endmeasure_expect sum(forecast_weight(1,:).*measure_point,2);P_z forecast_weight(2,:).*(measure_point-measure_expect)*...(measure_point-measure_expect)R;P_xz forecast_weight(2,:).*(forecast_point-forecast)*...(measure_point-measure_expect);kalman_gain P_xz/P_z;estimate forecast kalman_gain*(measure - measure_expect);cov_error_estimate P_forecast - kalman_gain*P_z*kalman_gain;end
end
end结果展示
异步无迹Kalman滤波
在目标状态估计过程中量测通常以离散化采样给出同时带有一个探测时间标志。分布式传感器网络是对多个量测进行处理。由于传感器网络传输存在随机的时间延迟且各个节点对于量测的预处理时间存在随机性很可能出现源于某个目标的较晚的量测到达某一节点后较早的量测才到达该节点这种情况就称为非顺序量测。在目标状态估计过程中传感器节点通常仅保存航迹的充分统计量。这样当接收到一个延迟的量测时假定该量测的时戳为t为了实时性估计结果被更新到tzt时刻此时需要使用来自时刻t的量测更新tz时刻的估计。这就是在实际的多传感器系统中常见的负时间量测更新问题原因是t-tz为负而在标准的滤波问题中总假定为非负。通常情况下的滤波算法不适用于这种情况因此就需要对适用于异步滤波的算法进行研究。
原理
首先我们需要对非线性目标运动模型以不同的时间间隔进行重新的离散化定义。对于任意时间间隔的状态转移矩阵为任意时间间隔的噪声系数矩阵为任意时间间隔的系统噪声为 本文仅研究高斯白噪声随机过程则根据随机积分的性质可知离散化后的系统噪声仍为均值为0的高斯白噪声其协方差矩阵为 为了便于描述假定当前时刻为t而最新的量测时刻为td且td的量测为t时刻的延迟量测也就是说
此时 其中d为td时刻的简化标记。可以得到 其中为的逆矩阵表示后向状态转矩矩阵。 我们要解决的问题为在t时刻已知目标的状态估计和估计误差协方差。同时我们得到了td时刻的量测。此时需要用td时刻的量测来更新t时刻状态估计和估计误差协方差即计算 其中。 在最小均方误差准则下对于非顺序量测问题的最优更新算法为
其中
为了求出我们需要得到Kd和的值。首先 其中
由传感器探测模型及最小均方误差准则得 可知 其中 的σ点为
根据传感器探测模型得 求得计算的σ点为 同理根据已知的求得计算的σ点为 计算量测的一步提前预测即 可知 此时因为等于0可得。由最小均方误差准则可得 带入式子可求。根据式子可得与 可得的σ点为 根据非线性变换可得的σ点为 由的σ点可求 可得Kd将Kd和的带入可得。接下来将和带入即可求出。至此完成了用td时刻的量测来更新t时刻状态估计和估计误差协方差。
代码及其结果展示
核心代码完整代码见文末
function [estimate, cov_error_estimate] unscented_aysn(measure_aysn,t_asyn,...x_last, P_last,flag,radar,last_est_last,last_P_last,measure)
%kalman_filter 扩展kalman滤波算法
% 输入量测值上一时刻估计上一时刻估计误差协方差,是否预测的标志,
%雷达的状态信息
% 输出当前时刻估计当前时刻估计误差协方差
% 当flag为1则跳过预测步骤
%%
global t_step;
% 状态转移矩阵
global A;
% 系统噪声系数矩阵
global gama;
% 系统噪声协方差
global Q;
% 量测噪声协方差
global R;
%%
last_est_last A * last_est_last;
last_P_last (A*last_P_last *A gama*Q* gama);
% 如果全零即无初始化
if ~any(x_last)if measure_aysn0estimate x_last;cov_error_estimate P_last;else% 如果是0时刻则取量测值对应的状态为当前时刻估计9倍的量测误%差协方差阵为当前时刻估计误差协方差。% 此处为9的原因是3倍的标准差为百分之95的可能的误差值可以认为%包含了最大误差的情况。estimate[radar(1,1)measure_aysn(1,1)*cos(measure_aysn(3,1))*cos(measure_aysn(2,1));radar(2,1)measure_aysn(1,1)*sin(measure_aysn(3,1));radar(3,1)-measure_aysn(1,1)*cos(measure_aysn(3,1))*sin(measure_aysn(2,1));0;0;0];cov_error_estimate 1*[1,0,0,1,0,0;0,1,0,0,1,0;0,0,1,0,0,1;1,0,0,1,0,0;0,1,0,0,1,0;0,0,1,0,0,1];end
elseif flag1forecastx_last;P_forecastP_last;elseforecast A * x_last;P_forecast (A*P_last *A gama*Q* gama);endif measure_aysn0estimateforecast;cov_error_estimate P_forecast;elsenum_state size(forecast,1);Q_td_t Q_t1_t2(t_asyn,t_step);[w_td2t_t_1_point,~] ...get_point(zeros(num_state,1), Q_td_t,3);[last_est_point,last_est_weight] ...get_point(last_est_last, last_P_last, 3);measure_t_1_point zeros(size(measure,1),size(last_est_point,2));for cou_point 1:size(last_est_point,2)measure_t_1_point(:,cou_point)measure_func(...last_est_point(1:3,cou_point)-radar(1:3,1));endmeasure_expect_t_1 sum(last_est_weight(1,:).*measure_t_1_point,2);cov_zt_t_1 last_est_weight(2,:).*(measure_t_1_point-...measure_expect_t_1)*(measure_t_1_point-measure_expect_t_1)R;% TODO 应该修正R后的pointcov_w_td2t_zt_t_1 last_est_weight(2,:).*(w_td2t_t_1_point)*...(measure_t_1_point-measure_expect_t_1);w_td2t cov_w_td2t_zt_t_1/cov_zt_t_1*(measure-...measure_expect_t_1);cov_w_td2t_t Q_td_t-cov_w_td2t_zt_t_1/cov_zt_t_1*...cov_w_td2t_zt_t_1;[forecast_point,forecast_weight]get_point(forecast,P_forecast,3);% (3-25)backcast A_t1_t2(t_step,t_asyn)*(forecast-w_td2t);P_backcast A_t1_t2(t_step,t_asyn)*P_forecast*...(A_t1_t2(t_step,t_asyn))-A_t1_t2(t_step,t_asyn)*...(cov_w_td2t_t)*(A_t1_t2(t_step,t_asyn));[backcast_point,backcast_weight]get_point(backcast,P_backcast,3);measure_expect_t_point zeros(size(measure,1),size(backcast_point,2));for cou_point 1:size(backcast_point,2)measure_expect_t_point(:,cou_point)measure_func(...backcast_point(1:3,cou_point)-radar(1:3,1));endmeasure_expect_t sum(backcast_weight(1,:).*measure_expect_t_point,2);% (3-36)P_z backcast_weight(2,:).*(measure_expect_t_point-...measure_expect_t)*(measure_expect_t_point-measure_expect_t)R;% (3-37) % TODO 应该修正R后的pointP_xz forecast_weight(2,:).*(forecast_point-forecast)*...(measure_expect_t_point-measure_expect_t);kalman_gain P_xz/P_z;estimate forecast kalman_gain*(measure_aysn - measure_expect_t);cov_error_estimate P_forecast - P_xz/P_z*P_xz;end
end
end结果展示 完整代码分为多个文件数量较多无法放在文章中。完整代码在公众号沸腾的火锅资源号中自取。
