【卡尔曼滤波器】

置顶浮光629

已于 2023-08-31 19:36:52 修改

阅读量2.1k

点赞数 2

文章标签： matlab 算法

于 2023-03-25 10:48:08 首次发布

本文链接：https://blog.csdn.net/asdjkj/article/details/124595372

版权

卡尔曼滤波器（观测器）

前言

本文参考了Matlab对卡尔曼滤波器的官方教程及帮助文档（Kalman Filter）。另外提供友情链接如下：

参考连接：https://zhuanlan.zhihu.com/p/338269917

参考链接：

一、卡尔曼观测器

卡尔曼滤波器（Kalman filter）或者说卡尔曼观测器是指利用卡尔曼滤波算法间接估算测量值的一种最优状态观测器。

1.从一个实例到全阶状态观测器

1.1一个简单的例子

以一维直线运动的小车为例，系统的状态为：

${x_1} = x$ ，小车位置

${x_2} = \dot x$ ，小车速度

加入控制量 $u$ ，可得系统状态方程：

${\bf{\dot x}} = \left[ {\begin{array}{*{20}{c}} 0&1\\ 0&0 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{x_1}}\\ {{x_2}} \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} 0\\ {\frac{1}{m}} \end{array}} \right]u$

令： ${\bf{A}} = \left[ {\begin{array}{*{20}{c}} 0&1\\ 0&0 \end{array}} \right]$ ， ${\bf{B}} = \left[ {\begin{array}{*{20}{c}} 0\\ {\frac{1}{m}} \end{array}} \right]$

则有： ${\bf{\dot x}} = {\bf{Ax}} + {\bf{Bu}}$ 。

输出假设为位置x，则有

${\bf{y}} = \left[ {\begin{array}{*{20}{c}} 1&0 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{x_1}}\\ {{x_2}} \end{array}} \right]$

令： ${\bf{C}} = \left[ {\begin{array}{*{20}{c}} 1&0 \end{array}} \right]$ ， ${\bf{D}} = 0$

则有： ${\bf{y = Cx + Du}}$ 。

即：

$\begin{array}{l} {\bf{\dot x}} = {\bf{Ax}} + {\bf{Bu}}\\ {\bf{y = Cx + Du}} \end{array}$

1.2全阶状态观测器设计

利用 ${\bf{\dot x}} = {\bf{Ax}} + {\bf{Bu}}$ 和初始状态 ${{\bf{x}}_0}$ 可以估计 ${\bf{\hat x}}$ ，然而存在误差，借助反馈进行消除。

假设系统输出的测量值为 ${\bf{z}}(t)$

设计状态观测器：

${\bf{\dot {\hat x}}} (t)= {\bf{Ax}}(t) + {\bf{Bu}}(t)$

加入补偿增益

${\bf{\dot {\hat x}}}(t) = {\bf{Ax}}(t) + {\bf{Bu}}(t) + {\bf{L}}({\bf{z}}(t) - {\bf{Cx}}(t))$

1.3离散化

将上式离散化可得

${{\bf{\hat x}}_{k + 1}} = {{\bf{\hat x}}_k} + \Delta t({\bf{A}}{{\bf{\hat x}}_k} + {\bf{B}}{{\bf{u}}_k} + {\bf{L}}({{\bf{z}}_k} - {\bf{C}}{{\bf{\hat x}}_k}))$

离散化后的状态观测器存在问题： $k + 1$ 时刻的状态值是由 ${{\bf{z}}_k} - {\bf{C}}{{\bf{\hat x}}_k}$ 得到的。

为解决这一问题将上述方程拆为两个：

预测方程： ${{\bf{\hat x}}_{k + 1|k}} = {\bf{F}}{{\bf{x}}_{k|k}} + {{\bf{B}}^{\bf{ - }}}{{\bf{u}}_k}$ （1）

修正方程： ${{\bf{\hat x}}_{k + 1|k + 1}} = {{\bf{\hat x}}_{k + 1|k}} + {\bf{K}}({{\bf{z}}_{k + 1}} - {\bf{C}}{{\bf{\hat x}}_{k + 1|k}})$ （2）

令估计误差为： ${{\bf{e}}_k} = {{\bf{x}}_k} - {{\bf{\hat x}}_k}$ ，问题转变为求矩阵 ${\bf{K}}$ 的值使 ${{\bf{e}}_k}$ 随时间收敛至 ${\bf{0}}$ 。

2.卡尔曼观测器

考虑噪声，重新定义预测方程和修正方程：

$\bf{\hat x}_{k|k - 1}={{\bf{F}}_k}{{\bf{x}}_{k - 1|k - 1}}+ {{\bf{B}}_k}{{\bf{u}}_{k - 1}}+{{\bf{w}}_{k - 1}},{{\bf{w}}_k}\sim {\bf{N}}(\bf{0}_{n\times 1},{{\bf{Q}}_k})$

${{\bf{\hat x}}_{k|k}} = {{\bf{\hat x}}_{k|k - 1}} + {{\bf{K}}_k}({{\bf{z}}_k} - {{\bf{H}}_k}{{\bf{\hat x}}_{k|k - 1}})$

重新定义估计误差 ${{\bf{e}}_{k|k}} = {{\bf{x}}_k} - {{\bf{\hat x}}_{k|k}}$ ，由于模型误差和观测误差的存在，计算中无法得到其真实状态 ${\bf{x}}$ ，假设 ${{\bf{e}}_{k|k}}$ 服从正态分布，若得到最优增益 ${\bf{e}}_{k|k}\sim {\bf{N}}({{\bf{0}}_{m \times 1}},{P_{k|k}})$ ，则使估计误差 ${{\bf{e}}_{k|k}}$ 的均方差最小。

${{\bf{P}}_{k|k}} = {\bf{E}}[{{\bf{e}}_{k|k}}{{\bf{e}}_{k|k}}^T] = {\bf{E}}[(({\bf{I}} - {{\bf{K}}_k}{{\bf{H}}_k})({{\bf{x}}_k} - {{\bf{\hat x}}_{k|k - 1}}) - {{\bf{k}}_k}{{\bf{v}}_k}){(({\bf{I}} - {{\bf{K}}_k}{{\bf{H}}_k})({{\bf{x}}_k} - {{\bf{\hat x}}_{k|k - 1}}) - {{\bf{k}}_k}{{\bf{v}}_k})^T}]$

定义先验误差 ${{\bf{e}}_{k|k - 1}} = {{\bf{x}}_k} - {{\bf{\hat x}}_{k|k - 1}}$ ，服从正态分布 ${{\bf{e}}_{k|k - 1}}\sim {\bf{N}}({{\bf{0}}_{m \times 1}},{P_{k|k - 1}})$

可得

$\begin{align*} {{\bf{P}}_{k|k}} &= {\bf{E}}[({\bf{I}} - {{\bf{K}}_k}{{\bf{H}}_k}){{\bf{e}}_{k|k - 1}}{{\bf{e}}_{k|k - 1}}^T{({\bf{I}} - {{\bf{K}}_k}{{\bf{H}}_k})^T}] - {\bf{E}}[{{\bf{k}}_k}{{\bf{v}}_k}{{\bf{e}}_{k|k - 1}}^T{({\bf{I}} - {{\bf{K}}_k}{{\bf{H}}_k})^T}]\\ &- {\bf{E}}[({\bf{I}} - {{\bf{K}}_k}{{\bf{H}}_k}){{\bf{e}}_{k|k - 1}}{{\bf{v}}_k}^T{{\bf{k}}_k}^T] + {\bf{E}}[{{\bf{k}}_k}{{\bf{v}}_k}{{\bf{v}}_k}^T{{\bf{k}}_k}^T] \end{align*}$

${{\bf{v}}_k}$ 与 ${{\bf{e}}_{k|k - 1}}^T$ 相互独立，且 ${\bf{E}}[{{\bf{v}}_k}] = 0,{\bf{E}}[{{\bf{e}}_{k|k - 1}}^T] = 0$

则 ${{\bf{P}}_{k|k}} = ({\bf{I}} - {{\bf{K}}_k}{{\bf{H}}_k}){{\bf{P}}_{k|k - 1}}{({\bf{I}} - {{\bf{K}}_k}{{\bf{H}}_k})^T}] - {{\bf{k}}_k}{{\bf{R}}_k}{{\bf{k}}_k}^T$

问题转变为求取合适的 ${{\bf{k}}_k}$ 使 $tr({{\bf{P}}_{k|k}})$ 最小。

$tr({{\bf{P}}_{k|k}})$ 对 ${{\bf{k}}_k}$ 求导可得：

$\frac{{dtr({{\bf{P}}_{k|k}})}}{{d{{\bf{K}}_k}}} = 0 - 2{{\bf{P}}_{k|k - 1}}^T{{\bf{H}}_k}^T + 2{{\bf{K}}_k}{{\bf{H}}_k}{{\bf{P}}_{k|k - 1}}{{\bf{H}}_k}^T + 2{{\bf{K}}_k}{{\bf{R}}_k} = 0$

（注 $\frac{{dtr({\bf{AB}})}}{{d{\bf{A}}}} = {{\bf{B}}^T},\frac{{dtr({\bf{AB}}{{\bf{A}}^{\rm{T}}})}}{{d{\bf{A}}}} = 2{\bf{AB}}$ ）

解出： ${{\bf{K}}_k} = {{\bf{P}}_{k|k - 1}}{{\bf{H}}_k}^T{({{\bf{H}}_k}{{\bf{P}}_{k|k - 1}}{{\bf{H}}_k}^T + {{\bf{R}}_k})^{ - 1}}$ （3）

目前 ${{\bf{P}}_{k|k - 1}}$ 未知

${{\bf{e}}_{k|k - 1}} = {{\bf{x}}_k} - {{\bf{\hat x}}_{k|k - 1}}$

将预测方程 ${{\bf{\hat x}}_{k|k - 1}} = {{\bf{F}}_k}{{\bf{x}}_{k - 1|k - 1}} + {{\bf{B}}_k}{{\bf{u}}_{k - 1}} + {{\bf{w}}_{k - 1}},{{\bf{w}}_k}\sim {\bf{N}}({{\bf{0}}_{n \times 1}},{{\bf{Q}}_k})$ 带入可得：

${{\bf{e}}_{k|k - 1}} = {{\bf{F}}_k}{{\bf{e}}_{k - 1|k - 1}} + {{\bf{w}}_{k - 1}}$

可得

$\begin{align*} {{\bf{P}}_{k|k - 1}} &= {\bf{E}}[{{\bf{e}}_{k|k - 1}}{{\bf{e}}_{k|k - 1}}^T]\\&= {\bf{E}}[{{\bf{F}}_k}{{\bf{e}}_{k - 1|k - 1}}{{\bf{e}}_{k - 1|k - 1}}^T{{\bf{F}}_k}^T] + {\bf{E}}[{{\bf{w}}_{k - 1}}{{\bf{e}}_{k - 1|k - 1}}^T{{\bf{F}}_k}^T] \\&+ {\bf{E}}[{{\bf{F}}_k}{{\bf{e}}_{k - 1|k - 1}}^T{{\bf{w}}_{k - 1}}^T] + {\bf{E}}[{{\bf{w}}_{k - 1}}{{\bf{w}}_{k - 1}}^T] \end{align*}$

有 ${{\bf{w}}_{k - 1}}$ 与 ${{\bf{e}}_{k - 1|k - 1}}^T$ 相互独立，故

${{\bf{P}}_{k|k - 1}}{\rm{ = }}{{\bf{F}}_k}{{\bf{P}}_{k - 1|k - 1}}{{\bf{F}}_k}^T + {{\bf{Q}}_{k - 1}}$ （4）

将 ${{\bf{K}}_k} = {{\bf{P}}_{k|k - 1}}{{\bf{H}}_k}^T{({{\bf{H}}_k}{{\bf{P}}_{k|k - 1}}{{\bf{H}}_k}^T + {{\bf{R}}_k})^{ - 1}}$ 带入

${{\bf{P}}_{k|k}} = ({\bf{I}} - {{\bf{K}}_k}{{\bf{H}}_k}){{\bf{P}}_{k|k - 1}}{({\bf{I}} - {{\bf{K}}_k}{{\bf{H}}_k})^T}] - {{\bf{k}}_k}{{\bf{R}}_k}{{\bf{k}}_k}^T$ 可得：

${{\bf{P}}_{k|k}} = ({\bf{I}} - {{\bf{K}}_k}{{\bf{H}}_k}){{\bf{P}}_{k|k - 1}}$ （5）

至此卡尔曼滤波器黄金五条推导完成（公式（1）（2）（3）（4）（5））。

二、卡尔曼观测器的设计

1.卡尔曼滤波过程总结

步骤1：获取系统状态方程和观测方程

步骤2：初始化系统状态（手动给定）、初始协方差矩阵 $\bf P_{k|k}$

步骤3：根据 ${{\bf{\hat x}}_{k }}$ 预测 ${{\bf{\hat x}}_{k+1}}$ ， ${{\bf{\hat x}}_{k + 1|k}} = {\bf{F}}{{\bf{x}}_{k|k}} + {{\bf{B}}^{\bf{ - }}}{{\bf{u}}_k}$

步骤4：计算卡尔曼增益 ${\bf {K_k}} = \frac{{\bf P_{k|k}}{\bf H^T}}{{\bf H}{\bf P_{k|k}}{\bf H^T}+\bf R}$

步骤5：校正：根据观测值和卡尔曼增益对预测值进行校正，得到估计值 ${\bf \hat{x}_{k+1|k+1}}={\bf \hat{x}_{k+1|k}}+{\bf K_k}({\bf z_k}-{\bf H}{\bf \hat{x}_{k+1|k}})$

步骤:6：计算协方差矩阵 ${\bf P_{k+1|k}}=({\bf I}-{\bf K_k}{\bf H}){\bf {P_{k|k}}}$

步骤7：更新协方差矩阵 ${\bf P_{k+1|k+1}}={\bf F}{\bf P_{k|k}}{\bf F^T}+{\bf Q_{k}}$

步骤8：重复3、4、5、6、7

顺序如下：

2.代码实现（matlab）

代码如下（示例）：

clc;
clear all;
%假设一辆汽车在做匀速直线运动（一维场景），驾驶员通过油门控制了汽车的加速度恒定。
%忽略汽车所受的阻力、质量变化，并假设驾驶员操作会给汽车的牵引力造成一定的过程噪声。
%选择汽车的位移和速度为状态变量x = [p,v].'，则状态变量导数为汽车的速度和加速度即
%x'= [v,a].'，选取控制变量u=a，测量量为汽车的位移。
%则状态方程如下：

%系统状态空间方程
A=[0 1;0 0];
B=[0;1];
C=[0 1];
D=0;
sys_time=ss(A,B,C,D);

%求系统阶数
n=size(sys_time.A,1);

%系统采样时间
delta_t=0.1;%采样间隔

%求系统离散模型
F_k=eye(n)+delta_t*A;
B_k=delta_t*B;
H_k=C;

%仿真次数=仿真时间/采样时间
N = 100;  % 仿真数

%定义系统扰动
%Q为w的方差，R为v的方差，假设w与v相互独立
Q = diag([0,0.05]);
R = 1;
%w为控制变量的随机扰动，v为测量的随机扰动
w = sqrt(Q)*randn(n,N); % 控制变量的误差2*N
v = sqrt(R)*randn(size(sys_time.C,1),N); % 测量误差1*N

%P初始值
P = Q;

%给定初始状态
x0 = [0.2;0];  % 初始位置和速度
xh0 = [0;0];  % x0的估计
u = ones(1,N);  % 加速度恒定

%卡尔曼滤波器初始状态
ye_list = zeros(size(u));  % 估计值
yv_list = zeros(size(u));  % 测量值
y_list = zeros(size(u));  % 实际值
cov_list = zeros(size(u));  % 测量方差
k_list = zeros([2 100]);

%卡尔曼滤波过程
I=eye(n);
for i = 1:numel(u)
    %获取真实值
    xreal = F_k*x0 + B_k*u(i);  % 真实的状态变量
    yreal = H_k*x0;  % 真实的测量
    %x1 = xreal + w(:,i);  % 含噪声的状态变量
    x1 = xreal;  % 含噪声的状态变量
    yv = yreal + v(:,i);  % 含噪声的测量
    
    %求x,P的先验估计值
    xfe = F_k*xh0 + B_k*u(i);  % 先验的状态变量
    Pfe = F_k*P*F_k'+ Q;  % 先验的状态变量协方差
    
    %求卡尔曼增益
    K = Pfe*H_k'/(H_k*Pfe*H_k'+R);  % 卡尔曼最优增益
    k_list(:,i) = K;
    
    xh1 = xfe + K*(yv-H_k*xfe);  % 当前的状态估计
    ye = H_k*xh1;% 当前输出估计值
    
    %更新协方差矩阵
    P = (I-K*H_k)*Pfe;
    %更新初始状态和估计值
    x0 = x1;
    xh0 = xh1;
    
    %存入数据
    y_list(i) = yreal;
    yv_list(i) = yv;
    ye_list(i) = ye;
    cov_list(i) = C*P*C';
end

ax = (1:N).*delta_t;%横坐标

figure(1);
subplot(2,2,1)
plot(ax,y_list,ax,yv_list,ax,ye_list)
legend('实际','测量','估计','Location','best')
title('汽车的位移')
ylabel('位移/m')
xlabel('时间/s')

subplot(2,2,2)
plot(ax,yv_list-y_list,ax,ye_list-y_list)
legend('测量','估计','Location','best')
title('汽车的位移误差')
ylabel('位移/m')
xlabel('时间/s')

subplot(2,2,[3,4])
plot(ax,cov_list)
legend('测量方差','Location','best')
title('测量方差')
ylabel('方差/m^2')
xlabel('时间/s')

%k值逐渐收敛
figure(2);
plot(ax,k_list)

3.运行结果

三、无迹卡尔曼

1.无迹卡尔曼原理

无迹卡尔曼滤波是基于无迹变换和标准卡尔曼滤波器的结合，可以用于非线性系统。UKF通过生成一些点（sigma点集），来近似非线性。由这些点来决定实际x和P的取值范围。

假设一个非线性系统 $y = f(x)$ ，其中 $x$ 为 $n$ 维状态向量，并已知其平均值为 $\bar x$ ，方差为 ${{\bf{P}}_x}$ ，则可以经过UT变换构造 $2n + 1$ 个Sigma点 ${x_i}$ ，同时构造相应的权值 ${{\bf{W}}_i}$ ，进而得到 $y$ 的统计特性（均值和方差）。

假设系统的状态方程和观测方程为：

$\begin{array}{l} {\bf{x}}(k + 1) = f({\bf{x}}(k),{\bf{w}}(k))\\ {\bf{z}}(k) = h({\bf{x}}(k),{\bf{v}}(k)) \end{array}$

无极卡尔曼滤波器实现步骤如下：

步骤1：初始化系统状态 ${x_k}$ ， ${{\bf{P}}_k}$

步骤2：根据状态 ${x_k}$ ， ${{\bf{P}}_k}$ 生成Sigma点 ${{\bf{X}}_k}$

步骤3：根据预测模型预测未来的Sigma点 ${{\bf{X}}_{k + 1|k}}$

步骤4：根据预测的Sigma点 ${{\bf{X}}_{k + 1|k}}$ 生成状态预测的Sigma点 ${{\bf{X}}_{k + 1|k}}$ ， ${{\bf{P}}_{k + 1|k}}$

步骤5：当测量值到来时，将预测的Sigma点 ${{\bf{X}}_{k + 1|k}}$ 转换成预测测量值 ${{\bf{{\rm Z}}}_{k + 1|k}}$

步骤6：根据预测测量值 ${{\bf{{\rm Z}}}_{k + 1|k}}$ 与真实测量值 ${{\bf{z}}_{k + 1}}$ 的差值更新得到系统状态 ${{\bf{X}}_{k + 1|k + 1}}$ ， ${{\bf{P}}_{k + 1|k + 1}}$ （两个高斯分布相乘得到新的系统状态 ${{\bf{X}}_{k + 1|k + 1}}$ ， ${{\bf{P}}_{k + 1|k + 1}}$ ）

2.matlab代码

转至：

转至：无迹卡尔曼滤波UKF的理解与应用（附Matlab实例）

clc;
clear all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%过程分析%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%使用UKF的前提：你得有状态方程与观测方程
% X(k+1)=f(x(k),W(k))
% Z(k)=h(x(k),V(k))

%常用于无人驾驶领域，所用的模型有：
%恒定速度模型（Constant Velocity, CV）
%恒定加速度模型（Constant Acceleration, CA）
%恒定转率和速度模型（Constant Turn Rate and Velocity，CTRV）
%恒定转动率和加速度（CTRA）
%恒定转向角和速度（CSAV）
%恒定曲率和加速度（CCA）
%及有确定的状态方程和观测方程

%以CTRA模型为例，一般形式无迹变换。
%不考虑过程噪声影响系统状态方程为：
%(k_a = k-1)
%X_k=X_k_a+[v/w(sin(u_k_a+w*dt)-sin(u_k_a))
%          v/w(-cos(u_k_a+w*dt)+cos(u_k_a))
%                       0                 
%                      w*dt        
%                       0                  ]
%过程噪声q_a~N(0,Q_a),q_w'~N(0,Q_w')
%        Q=[q_a
%           q_w']
%忽略q_w'对位移的影响
%最终系统的状态方程为：
%X_k=X_k_a+[v_k_a/w_k_a(sin(u_k_a+w*dt)-sin(u_k_a))           
%          v_k_a/w_k_a(-cos(u_k_a+w*dt)+cos(u_k_a))
%                       0                                  
%                      w*dt        
%                       0                         ]

%                       +

%[0.5dt^2*q_ak*cos(u_k_a)
% 0.5dt^2*q_ak*sin(u_k_a)
%         dt*q_ak
%      0.5dt^2*q_w'k
%        dt*q_w'k        ]


%观测过程Z_k=h(X_k)+R_k
%Z_k=[sqrt(p_xk^2+p_yk^2)
%      arctan(p_yk/p_xk)
%     (v_kcos(u_k)p_xk+v_ksin(u_k)p_yk)/sqrt(p_xk^2+p_yk^2)]

%                       +

%[r_pk
% r_uk
% r_u'k]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%代码实现%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%初始化%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%
% UKF在目标跟踪中的应用
%%%
clc;clear
T=1;
N=60/T;
X=zeros(4,N);%目标真实位置、速度，
X(:,1)=[-100,2,200,20];
Z=zeros(1,N);
delta_w=1e-3;
Q=delta_w*diag([0.5,1]);%过程噪声均值
G=[T^2/2,0;T,0;0,T^2/2;0,T];%过程噪声驱动矩阵
R=5;%观测噪声方差
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];%状态转移矩阵
x0=200;%观测站位置
y0=300;
Xstation=[x0,y0];%雷达站位置
%%%%%%%%%%%%%%
v=sqrtm(R)*randn(1,N);
for t=2:N
    X(:,t)=F*X(:,t-1)+G*sqrtm(Q)*randn(2,1);%目标真实轨迹，含噪声
end
for t=1:N
    Z(t)=Dist(X(:,t),Xstation)+v(t);%对目标观测，目标与雷达站的距离，含噪声
end
%%%%%%%%%%%%%
%UKF滤波，UT变换
L=4;%4个变量
alpha=1;
kalpha=0;
belta=2;
ramda=3-L;
for j=1:2*L+1
    Wm(j)=1/(2*(L+ramda));
    Wc(j)=1/(2*(L+ramda));
end
Wm(1)=ramda/(L+ramda);%权值Wm的初值需要另外定
Wc(1)=ramda/(L+ramda)+1-alpha^2+belta;%权值Wc的初值需要另外定
%%%%%%%%%%%%%%%%
Xukf=zeros(4,N);
Xukf(:,1)=X(:,1);%把X真值的初次数据赋给Xukf
P0=eye(4);%协方差阵初始化
for t=2:N
    xestimate=Xukf(:,t-1);%获取上一步的UKF点
    P=P0;%协方差阵
    %第一步：获得一组Sigma点集
    cho=(chol(P*(L+ramda)))';
    for k=1:L
        xgamaP1(:,k)=xestimate+cho(:,k);
        xgamaP2(:,k)=xestimate-cho(:,k);
    end
    Xsigma=[xestimate,xgamaP1,xgamaP2];%xestimate是上一步的点，相当于均值点
    %第二步：对Sigma点集进行一步预测
    Xsigmapre=F*Xsigma;%F是状态转移矩阵
    %第三步：利用第二步的结果计算均值和协方差
    Xpred=zeros(4,1);
    for k=1:2*L+1
        Xpred=Xpred+Wm(k)*Xsigmapre(:,k);%均值
    end
    Ppred=zeros(4,4);
    for k=1:2*L+1
        Ppred=Ppred+Wc(k)*(Xsigmapre(:,k)-Xpred)*(Xsigmapre(:,k)-Xpred)';%协方差矩阵
    end
    Ppred=Ppred+G*Q*G';
    %第四步：根据预测值，再一次使用UT变换，得到新的sigma点集
    chor=(chol((L+ramda)*Ppred))';
    for k=1:L
        XaugsigmaP1(:,k)=Xpred+chor(:,k);
        XaugsigmaP2(:,k)=Xpred-chor(:,k);
    end
    Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2];
    %第五步：观测预测
    for k=1:2*L+1
        Zsigmapre(1,k)=hfun(Xaugsigma(:,k),Xstation);
    end
    %第六步：计算观测预测均值和协方差
    Zpred=0;
    for k=1:2*L+1
        Zpred=Zpred+Wm(k)*Zsigmapre(1,k);
    end
    Pzz=0;
    for k=1:2*L+1
        Pzz=Pzz+Wc(k)*(Zsigmapre(1,k)-Zpred)*(Zsigmapre(1,k)-Zpred)';
    end
    Pzz=Pzz+R;
    
    Pxz=zeros(4,1);
    for k=1:2*L+1
        Pxz=Pxz+Wc(k)*(Xaugsigma(:,k)-Xpred)*(Zsigmapre(1,k)-Zpred)';
    end
    %第七步：计算kalman增益
    K=Pxz*inv(Pzz);
    %第八步：状态和方差更新
    xestimate=Xpred+K*(Z(t)-Zpred);
    P=Ppred-K*Pzz*K';
    P0=P;
    Xukf(:,t)=xestimate;
end

%误差分析
for i=1:N
    Err_KalmanFilter(i)=Dist(X(:,i),Xukf(:,i));
end
%%%%%%%%%%%
%画图
figure
hold on ;box on
plot(X(1,:),X(3,:),'-k.');
plot(Xukf(1,:),Xukf(3,:),'-r+');
legend('真实轨迹','UKF轨迹')
figure
hold on ; box on
plot(Err_KalmanFilter,'-ks','MarkerFace','r')

%%%%%%%%%%%%%
%子函数
function d=Dist(X1,X2)
if length(X2)<=2
    d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
else
    d=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
end
end
function [y]=hfun(x,xx)
y=sqrt((x(1)-xx(1))^2+(x(3)-xx(2))^2);
end
%%%%%%%%%%%%%

3.运行结果

总结

以上就是今天要讲的内容，本文仅仅简单介绍了卡尔曼的原理及使用，并扩展了可以用于非线性系统的无迹卡尔曼观测器。

浮光629

关注

2
点赞
踩
25

收藏

觉得还不错? 一键收藏
0
评论
【卡尔曼滤波器】

本文参考了Matlab对卡尔曼滤波器的官方教程及帮助文档（Kalman Filter）。另外提供友情链接如下：步骤1：获取系统状态方程和观测方程步骤2：初始化系统状态（手动给定）、初始协方差矩阵步骤3：根据预测步骤4：计算卡尔曼增益步骤5：校正：根据观测值和卡尔曼增益对预测值进行校正，得到估计值步骤:6：计算协方差矩阵步骤7：更新协方差矩阵步骤8：重复3、4、5、6、7以上就是今天要讲的内容，本文仅仅简单介绍了卡尔曼的原理及使用，并扩展了可以用于非线性系统的无迹卡尔曼观测器。u。
复制链接

扫一扫