1 matlab
% ---------机载卫通角度自跟踪研究
% 1、目标捕获。 目的：获得目标大致的俯仰角theta0
% 2、单脉冲测角。目的：获得目标精准的俯仰角thetas
% 3、卡尔曼滤波。目的：获得滤波后的角度thetas_est
close all;
clear all;
clc;
%----------------参数设置
% 主函数相关参数
T=1; % 滤波周期
M=100; % 仿真步数
% 目标捕获函数相关参数
c=3e8; % 光速
f=3e9; % 信号频率
fs=20*f; % 采样频率
N=8; % 阵元个数
bujing=1; % 扫描步进
count=6000; % 采样点数
SNR=-20; % 信噪比
% 单脉冲测角相关参数
bujing1=1; % 扫描步进
SNR1=-20; % 信噪比
count1=3000;
thetak=0.5*(100/N);% 子波束与跟踪轴的夹角
% 卡尔曼滤波函数相关参数
% TT=[T*T/2,0,0;T,0,0;0,T*T/2,0;0,T,0;0,0,T*T/2;0,0,T]; %状态噪声的输入矩阵(XYZ)
TT=[T*T/2,0;T,0;0,T*T/2;0,T]; % 状态噪声的输入矩阵(XY)
Q1=0.1*eye(2); % 两个方向的伪加速度
Q=TT*Q1*TT'; % 系统状态噪声的方差 因为目标的状态在变化，所以不可以精确建模，故有状态噪声
% Q=10*eye(4) ; % 系统状态噪声的方差 因为目标的状态在变化，所以不可以精确建模，故有状态噪声
Error_est =10; % 测量误差的方差；
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 设定目标运动轨迹
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
X=[]; % 目标水平方向的真实位置、速度;垂直方向的的真实位置
r=[]; % 目标与原点的距离
X0 = 2000;
Y0 = 5000;
Vx = 300;
Vy = 200;
Y(:,1)=[X0,Vx,Y0,Vy]; % 目标初始位置为（100,100）、初始速度为（200,100） x,vx,y,vy
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];% 状态转移矩阵
w=sqrt(Q)*randn(4,M); % 在匀速运动轨迹上叠加噪声
for p=2:M
Y(:,p)=F*Y(:,p-1)+w(:,p); % 目标运动轨迹方程
% Y(:,p)=F*Y(:,p-1); % 目标运动轨迹方程
r(p)=sqrt(Y(1,p)^2+Y(3,p)^2);
theta_true(p)=acos(Y(3,p)/r(p))*180/pi;
end
% 循环从2开始，所以需要下面两行代码
r(1)=sqrt(Y(1,1)^2+Y(3,1)^2);
theta_true(1)=acos(Y(3,1)/r(1))*180/pi; % 将位置换成角度
% figure(1)
% plot(Y(1,:),Y(3,:)); grid on;
% xlabel('X坐标/m');ylabel('Y坐标/m');
% % title('目标的运动轨迹');
% figure(2)
% plot(theta_true); grid on;
% xlabel('迭代次数');ylabel('角度');
% % title('目标角度的运动轨迹');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 第零次迭代
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%------------初始化 初始值越接近于真值，则收敛越快
% X_pre=[sind(theta_true(1)),cosd(theta_true(1)),cosd(theta_true(1)),-sind(theta_true(1))]'; % 飞机初始位置和速度 相当于X（0,0）
% X_est=[sind(theta_true(1)),(sind(theta_true(2))-sind(theta_true(1)))/T,cosd(theta_true(1)),(cosd(theta_true(2))-cosd(theta_true(1)))/T]';
% P_pre=1e-2*eye(4); % 状态误差自相关矩阵的估计值的初始值 相当于P（0,0）
%------------预测
% X_pre=F*X_est; % 预测下一时刻飞机的位置和速度 相当于X（1,0）
% X_pre=X_est; % 预测下一时刻飞机的位置和速度 相当于X（1,0）
% P_pre=P_est; % 状态误差自相关矩阵预测方程。相当于P（1,0）
%%%%%%%%测量值%%%%%%%%%%%%%%%%%
% theta0 = theta_true + sqrt(Error_est)*randn(1,M); % 第一次的测量值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 函数功能：测量目标大致角度
% 调用函数： 目标捕获
% 输入参数: 目标运动轨迹角度theta_ture
% 输出参数： 粗捕获得到的目标大致角度theta0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
theta0=target_get(theta_true(1),c,f,fs,N,bujing,count,SNR);% 跟踪轴角度 每次搜索更新跟踪轴角度
for p=1:M
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 调用函数：单脉冲测角
% 输入参数：跟踪轴角度theta0，目标偏离跟踪轴的角度thetat
% 输出参数：单脉冲测角得到的目标真实角度thetas
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if (p==1)
thetat(p)=theta_true(p)-theta0; % 用粗捕获的角度作为跟踪轴
thetas(p)=dmc_angle_esti(theta0,thetat(p),c,f,fs,N,bujing1,count1,SNR1,thetak);% 调用单脉冲测角函数
else
thetat(p)=theta_true(p)-thetas(p-1); % 用上一次的单脉冲测角角度作为跟踪轴
thetas(p)=dmc_angle_esti(thetas(p-1),thetat(p),c,f,fs,N,bujing1,count1,SNR1,thetak);% 调用单脉冲测角函数
end
end
% 5次
% thetas=[21.5076548036461,23.9035640695852,25.2549934363390,26.9359550143116,28.8976558818146];
% 100次
thetas=[21.5076548036461,23.9035640695852,25.2549934363390,26.9359550143116,28.8976558818146,31.0009336978619,30.8565778509341,32.3050100914770,34.1862053875184,34.6904798952295,35.6555170269539,36.8713678226845,37.1700434897918,37.8389043614815,37.8665872036294,39.7945125094379,39.6106898841655,40.4003792934401,41.4180428538889,41.4790716187582,41.9403468544136,42.9627346703708,42.8729673509037,43.4776654550822,43.7688342799448,43.1743927104658,44.2316718831226,44.4622411398221,44.4357070013545,45.3733188602618,45.5381975917392,46.5509298322948,45.6779847577037,47.1478396968443,46.2527226765848,46.3331447782597,47.4674626156377,45.8765540678496,47.1034394976255,46.8513567531198,48.6420380019551,48.8216730355858,48.0296108609148,48.3289066445719,49.1132668391982,48.0066000039878,48.7820375861355,48.6914931956388,48.6337594800962,48.6157152073843,49.4954097429769,49.9225918920769,49.2825002645643,49.5208738839231,50.2002660345929,49.5801729922124,50.9578089119918,48.6497288186680,50.2503630361635,49.4157770529638,50.6370793324727,50.9491032882535,51.2537774460507,51.3993217927973,49.6599949628813,50.1393171940319,51.0485148999235,51.2377563456300,51.3944425778218,50.3739007440455,51.3243416853895,51.1267954929477,51.0013308184272,50.8653367318906,52.7125778551996,51.3631307431436,51.5294521250658,52.0227598624784,51.5894953246860,51.7960221122716,50.7512572053516,51.4269769710341,52.1261355228410,51.3940536344455,51.3188234648857,52.3363148397478,52.3472901324124,51.3844978167153,51.6115223790734,52.5232396047085,53.1894031569837,53.3431758996074,54.2913611442235,52.8017240930785,51.7148643527885,51.6888832902782,52.5546540827346,52.7532214510904,53.6832072070126,53.7407670804744];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 第零次迭代
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%------------初始化 初始值越接近于真值，则收敛越快
X_pre=[sind(thetas(1)),cosd(thetas(1)),cosd(thetas(1)),-sind(thetas(1))]'; % 飞机初始位置和速度 相当于X（0,0）
P_pre=1e-2*eye(4); % 状态误差自相关矩阵的估计值的初始值 相当于P（0,0）
%------------预测
% X_pre=F*X_est; % 预测下一时刻飞机的位置和速度 相当于X（1,0）
% X_pre=X_est; % 预测下一时刻飞机的位置和速度 相当于X（1,0）
% P_pre=P_est; % 状态误差自相关矩阵预测方程。相当于P（1,0）
for p=1:M
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 函数功能：卡尔曼滤波 更新估计、预测
% 输入参数：状态预测值，估计误差预测值，目标捕获得到的角度,状态转移矩阵，状态噪声方差，测量噪声方差
% 输出参数：卡尔曼滤波的角度thetas_est，下一时刻的预测值，估计误差的预测值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[X_est(:,p),X_pre,P_pre] = KEM_tracking( X_pre,P_pre,thetas(p),F,Q,Error_est);
% theta_est(p) = atand(X_est(1,p)/X_est(3,p));
theta_est(p) = acos(X_est(3,p)/( sqrt( X_est(1,p)*X_est(1,p)+X_est(3,p)*X_est(3,p)) ))*180.0/pi;
% if (X_est(3,p)>1)% 如果角度的余弦值大于1，将估计角度固定为90度
% theta_est(p)=90;
% else
% theta_est(p) = acosd(X_est(3,p));
% end
end
for p=1:length(thetas) % 最后20次结果
fprintf('thetas[%g] is %g\n',p-1,thetas(p));
fprintf('theta_KEM[%g] is %g\n',p-1,theta_est(p));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 滤波平均角度误差计算
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% error=0;
% for p=1:50 % 最后20次结果
% error=error+abs(theta_est(M-50+p)-theta_true(M-50+p));
% end
% fprintf('滤波平均角度误差为%g\n',error/50);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 绘图
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% figure(3)
% plot(theta_true,'-r');hold on; % 目标真实轨迹的真实角度
% plot(thetas,'-bo');hold on; % 单脉冲测角得到的角度
% plot(theta_est,'-k*');grid on; % 卡尔曼滤波得到的目标角度
% legend('目标真实角度','单脉冲测角得到的角度','卡尔曼估计的角度');
% xlabel('跟踪步数');ylabel('角度/度');
% figure(4)
% plot(thetas-theta_true,'-bo');hold on; % 单脉冲测角得到的目标角度误差
% plot(theta_est-theta_true,'-k*');grid on; % 卡尔曼估计的角度误差
% legend('单脉冲测角得到的角度误差','卡尔曼估计的角度误差');
% xlabel('跟踪步数');ylabel('角度/度');
function [X_est,X_pre,P_pre] = KEM_tracking(X_pre,P_pre,theta_m,F,Q,Error_est)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 函数功能：卡尔曼滤波 更新估计、预测
% 输入参数：状态预测值，估计误差预测值，目标捕获得到的角度,状态转移矩阵，状态噪声方差，测量噪声方差
% 输出参数：卡尔曼滤波的角度thetas_est，下一时刻的预测值，估计误差的预测值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 观测方程的系数矩阵
H=[1,0,0,0;0,0,1,0];
% 观测值
Z=[sind(theta_m) cosd(theta_m)]';
% fprintf('Z(1)=%g\n',Z(1));
% fprintf('Z(2)=%g\n',Z(2));
%测量误差
% T = [cosd(theta_true) -sind(theta_true)]';
% R = T*[Error_est]*T';
R =Error_est*eye(2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 开始迭代过程（第一次迭代等等）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% for p=1:length(X_pre)
% fprintf('X_pre[%g] is %g\n',p,X_pre(p,1));
% end
% 更新
a=Z -H*X_pre; % 新息计算
% fprintf('a(1)=%g\n',a(1));
% fprintf('a(2)=%g\n',a(2));
A=H*P_pre*H'+R ; % 新息过程的自相关矩阵
% for p=1:2
% for q=1:2
% fprintf('A[%g][%g] is %g\n',p,q,A(p,q));
% end
% end
K=P_pre*H'*inv(A); % 计算卡尔曼增益
X_est=X_pre+K*a; % 状态估计
% 法一
% P_est=(eye(4)-K*H)*P_pre*(eye(4)-K*H)'+K*R*K'; % 估计误差自相关矩阵
% 法2
P_1=(eye(4)-K*H); % // P_1=(eye(4)-K*H) 维度：4*2 * 2*4=4*4
P_1_est= P_1*P_pre; % P_1_est=(eye(4)-K*H)*P_pre = P_1*P_pre
P_2_L=K*R; %P_2_L=K*R
P_2 =P_2_L*K';% P_2=K*R*K'=P_2_L*K'
P_est=P_1_est*(P_1)'+P_2; % 估计误差自相关矩阵
% 预测
X_pre = F*X_est ; % 下一时刻状态的预测值 相当于X（n+1,n）
P_pre = F*P_est*F'+Q; % 预测状态误差自相关矩阵 相当于P（n+1,n）
end
2 C语言
/* 函数功能：
卡尔曼滤波
*/
#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#define PI 3.1415926
#define COUNT 100
/*---------参数定义--------*/
int p;
int q;
int r;
int n;// 采样序列号
//theta_m的值表示MATLAB的测角结果
// 5次
//double theta_m[COUNT]={21.5076548036461,23.9035640695852,25.2549934363390,26.9359550143116,28.8976558818146};
// 100次
double theta_m[COUNT]={21.5076548036461,23.9035640695852,25.2549934363390,26.9359550143116,28.8976558818146,31.0009336978619,30.8565778509341,32.3050100914770,34.1862053875184,34.6904798952295,35.6555170269539,36.8713678226845,37.1700434897918,37.8389043614815,37.8665872036294,39.7945125094379,39.6106898841655,40.4003792934401,41.4180428538889,41.4790716187582,41.9403468544136,42.9627346703708,42.8729673509037,43.4776654550822,43.7688342799448,43.1743927104658,44.2316718831226,44.4622411398221,44.4357070013545,45.3733188602618,45.5381975917392,46.5509298322948,45.6779847577037,47.1478396968443,46.2527226765848,46.3331447782597,47.4674626156377,45.8765540678496,47.1034394976255,46.8513567531198,48.6420380019551,48.8216730355858,48.0296108609148,48.3289066445719,49.1132668391982,48.0066000039878,48.7820375861355,48.6914931956388,48.6337594800962,48.6157152073843,49.4954097429769,49.9225918920769,49.2825002645643,49.5208738839231,50.2002660345929,49.5801729922124,50.9578089119918,48.6497288186680,50.2503630361635,49.4157770529638,50.6370793324727,50.9491032882535,51.2537774460507,51.3993217927973,49.6599949628813,50.1393171940319,51.0485148999235,51.2377563456300,51.3944425778218,50.3739007440455,51.3243416853895,51.1267954929477,51.0013308184272,50.8653367318906,52.7125778551996,51.3631307431436,51.5294521250658,52.0227598624784,51.5894953246860,51.7960221122716,50.7512572053516,51.4269769710341,52.1261355228410,51.3940536344455,51.3188234648857,52.3363148397478,52.3472901324124,51.3844978167153,51.6115223790734,52.5232396047085,53.1894031569837,53.3431758996074,54.2913611442235,52.8017240930785,51.7148643527885,51.6888832902782,52.5546540827346,52.7532214510904,53.6832072070126,53.7407670804744};
double thetas_KEM[COUNT]={0};// 滤波角度
// 卡尔曼滤波相关参数
double X_pre[4][1]={{0},{0},{0},{0}};
double X_est[4][1]={{0},{0},{0},{0}};
double P_pre[4][4]={0};// 初始化
double T=1;// 滤波周期
double TT[4][2]={{T*T/2,0},{T,0},{0,T*T/2},{0,T}};
double TT_z[2][4]={ {T*T/2,T,0,0},{0,0,T*T/2,T} };
double Q1[2][2]={{0.1,0},{0,0.1}}; // 两个方向的伪加速度
double Q_temp[4][2]={0};// 中间变量
double Q[4][4]={0}; // 系统状态噪声的斜方差矩阵
double F[4][4]={{1,T,0,0},{0,1,0,0},{0,0,1,T},{0,0,0,1}};// 状态转移矩阵
// 现在测量的角度是在一个固定的角度上上下波动，相当于目标不动，所以此时速度为0
//double F[4][4]={{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};// 状态转移矩阵
double Error_est =10; //测量误差的方差；
/* 定义一个结构体，用来返回子函数的多个参数 */
typedef struct//结构体
{
double X_est[4][1];//状态估计值
double X_pre[4][1];//状态预测值
double P_pre[4][4];//估计误差协方差矩阵的预测值
}KEM_struct;// 声明了一个结构体 KEM_struct
// 结构体声明
KEM_struct KEM_back_parameter;// 卡尔曼滤波函数返回的参数
/*------------------子函数声明----------------*/
KEM_struct KEM_func(double X_pre[][1],double P_pre[][4],double thetam,double F[][4],double Q[][4],double Error_est);
/*-------------------主函数----------------*/
int main()
{
printf("beign\r\n");
/*---------1、得到观测值（模拟单脉冲测角的测角结果）-------
// 定义theta_m时已经产生
double rand_suijishu;//在真实值上叠加噪声
for (n=0;n<COUNT;n++)
{
rand_suijishu=rand() % 5 - 2 ;// 产生-2到2的随机数
theta_m[n]=rand_suijishu/10.0+10;// 真实值上叠加噪声
printf("theta_m[%d] is %.3f\r\n",n,theta_m[n]);
}*/
/*---------2、得到卡尔曼滤波所需的参数值--------*/
/* Q=TT*Q1*TT' 维度：4*4 */
//计算得到Q_temp=TT*Q1 维度：4*2
for (p=0;p<4;p++)
{
for (q=0;q<2;q++)
{
for (r=0;r<2;r++)
{
Q_temp[p][q]=Q_temp[p][q]+TT[p][r]*Q1[r][q] ;
}
}
}
//Q=TT*Q1*TT'=Q_temp*TT' 维度：4*4
for (p=0;p<4;p++)
{
for (q=0;q<4;q++)
{
for (r=0;r<2;r++)
{
Q[p][q]=Q[p][q]+Q_temp[p][r]*TT_z[r][q] ;
}
//printf("Q[%d][%d] is %.3f\r\n",p,q,Q[p][q]); // 经验证，正确
}
}
/*-----------3、初始化--------------*/
// 状态矢量初始化
X_pre[0][0]=sin(theta_m[0]*PI/180);
X_pre[1][0]=cos(theta_m[0]*PI/180);
X_pre[2][0]=cos(theta_m[0]*PI/180);
X_pre[3][0]=-sin(theta_m[0]*PI/180); // 飞机初始位置和速度 相当于X（0,0）
//printf("X_pre[0][0] is %.4f\r\n",X_pre[0][0]);
//printf("X_pre[1][0] is %.4f\r\n",X_pre[1][0]);
//printf("X_pre[2][0] is %.4f\r\n",X_pre[2][0]);
//printf("X_pre[3][0] is %.4f\r\n",X_pre[3][0]);// 经验证，正确
// 在定义P_pre的时候，已经对P_pre初始化
//P_pre=0.01*eye(4); // 状态误差自相关矩阵的估计值的初始值 相当于P（0,0）
for (p=0;p<4;p++)
{
for (q=0;q<4;q++)
{
if(p==q)
P_pre[p][q]=0.01;
else
P_pre[p][q]=0;
//printf("P_pre[%d][%d] is %.4f\r\n",p,q,P_pre[p][q]);// 经验证，正确
}
}
/*---------4、卡尔曼滤波--------*/
for (n=0;n<COUNT;n++)
{
KEM_back_parameter=KEM_func(X_pre,P_pre,theta_m[n],F,Q,Error_est);
for (p=0;p<4;p++)
{
X_est[p][0]=KEM_back_parameter.X_est[p][0] ;
X_pre[p][0]=KEM_back_parameter.X_pre[p][0] ;
}
for (p=0;p<4;p++)
{
for (q=0;q<4;q++)
{
P_pre[p][q]=KEM_back_parameter.P_pre[p][q] ;
}
}
//滤波后的角度
/*
if (X_est[2][0]>1.0)// 如果角度的余弦值大于1，将估计角度固定为90度
theta_filter[n]=90.0;
else
theta_filter[n] = acos(X_est[2][0])*180.0/PI;
*/
thetas_KEM[n] = acos(X_est[2][0]/( sqrt( X_est[2][0]*X_est[2][0]+X_est[0][0]*X_est[0][0] ) ))*180.0/PI;
//theta_filter[n] = atan(X_est[0][0]/X_est[2][0])*180.0/PI;
//printf("n is %d\r\n",n);
printf("thetas[%d] is %.4f\r\n",n,theta_m[n]);
printf("thetas_KEM[%d] is %.4f\r\n",n,thetas_KEM[n]);
}
return 0;
}
/*--------------子函数----------------*/
KEM_struct KEM_func(double X_pre[][1],double P_pre[][4],double thetam,double F[][4],double Q[][4],double Error_est)
{
double A1[2][4]={0};
double a[2][1]={0};
double A[2][2]={0};
double A_inv[2][2]={0};
double K[4][2]={0};
double K_z[2][4]={0};
double K1[4][2]={0};
double K_2L[4][2]={0};
double P_1[4][4]={0};
double P_1_est[4][4]={0};
double P_2_L[4][2]={0};
double P_2[4][4]={0};
double P_1_z[4][4]={0};
double P_est[4][4]={0};
double F_z[4][4]={0};
double F_P[4][4]={0};
/*----------中间变量清零--------------*/
for (p=0;p<2;p++)
{
for (q=0;q<2;q++)
{
A[p][q]=0;
A_inv[p][q]=0;
}
}
for (p=0;p<2;p++)
{
for (q=0;q<4;q++)
{
K_z[p][q]=0;
A1[p][q]=0;
}
}
for (p=0;p<4;p++)
{
for (q=0;q<2;q++)
{
K[p][q]=0;
K1[p][q]=0;
K_2L[p][q]=0;
P_2_L[p][q]=0;
}
}
for (p=0;p<4;p++)
{
for (q=0;q<4;q++)
{
P_1[p][q]=0;
P_1_est[p][q]=0;
P_2[p][q]=0;
P_1_z[p][q]=0;
P_est[p][q]=0;
F_z[p][q]=0;
F_P[p][q]=0;
}
}
// 观测方程的系数矩阵 维度：2*4
double H[2][4]={ {1,0,0,0}, {0,0,1,0}};
double H_z[4][2]={ {1,0},{0,0},{0,1},{0,0} };
// 观测值 维度：2*1
double Z[2][1]={{sin(thetam*PI/180.0)},{cos(thetam*PI/180.0)}}; // 经验证，正确
//printf(" Z[0][0] is %.4f\r\n", Z[0][0]);
//printf(" Z[1][0] is %.4f\r\n", Z[1][0]);
//测量误差
double R[2][2] ={{Error_est,0},{0,Error_est}};
/*---------开始迭代过程-----------*/
//for (p=0;p<4;p++)
//{
// printf("X_pre[%d][0] is %.4f\n",p,X_pre[p][0]);
//}
/*-------------更新---------------*/
// 新息计算 a=Z -H*X_pre 维度：2*1
a[0][0]=Z[0][0] -H[0][0]*X_pre[0][0];
a[1][0]=Z[1][0] -H[1][2]*X_pre[2][0];
//printf("a[0][0] is %.4f\r\n",a[0][0]);
//printf("a[1][0] is %.4f\r\n",a[1][0]);
// 新息过程的自相关矩阵 A=H*P_pre*H'+R; 维度：2*2
// 计算 A1=H*P_pre 维度：2*4*4*4=2*4
for (p=0;p<2;p++)
{
for (q=0;q<4;q++)
{
for(r=0;r<4;r++)
{
A1[p][q]=A1[p][q]+H[p][r]*P_pre[r][q];
}
}
}
// 计算 A=H*P_pre*H'+R=A1*H'+R
for (p=0;p<2;p++)
{
for (q=0;q<2;q++)
{
for(r=0;r<4;r++)
{
A[p][q]=A[p][q]+A1[p][r]*H_z[r][q]+R[p][q]/4.0;// 因为r循环了4次，所以要除以4
}
//printf("A[%d][%d] is %.4f\n",p,q,A[p][q]);
}
}
//printf("A[0][0] is %.4f\n",A[0][0]);
// 计算卡尔曼增益 K=P_pre*H'*inv(A); 维度：4*2
// 计算A_inv=inv(A) 维度：2*2
double A1_inv[2][2]={{A[1][1],-A[0][1]},{-A[1][0],A[0][0]}};//中间变量
for (p=0;p<2;p++)
{
for (q=0;q<2;q++)
{
A_inv[p][q]=A1_inv[p][q]/(A[0][0]*A[1][1]-A[1][0]*A[0][1]);
}
}
//计算 P_pre*H'=K1 维度：4*2
for (p=0;p<4;p++)
{
for (q=0;q<2;q++)
{
for(r=0;r<4;r++)
{
K1[p][q]=K1[p][q]+P_pre[p][r]*H_z[r][q];
}
}
}
//计算 K=P_pre*H'*inv(A)=K1 *inv(A) 维度：4*2
for (p=0;p<4;p++)
{
for (q=0;q<2;q++)
{
for(r=0;r<2;r++)
{
K[p][q]=K[p][q]+K1[p][r]*A_inv[r][q];// A_inv如何求解
}
//printf("K[%d][%d] is %.4f\n",p,q,K[p][q]);
}
}
//printf("K[0][0] is %.4f\n",K[0][0]);
// 状态估计 X_est[4]=X_pre[4]+K*a; 维度：4*2 * 2*1=4*1
double X1_est[4][1]={0};
for (p=0;p<4;p++)
{
X1_est[p][0]=K[p][0]*a[0][0]+K[p][1]*a[1][0];// 计算K*a=X1_est 维度：4*2 * 2*1=4*1
X_est[p][0]=X_pre[p][0]+X1_est[p][0];
//printf("X_est[%d][0] is %.4f\n",p,X_est[p][0]);
}
// 估计误差自相关矩阵 P_est=(eye(4)-K*H)*P_pre *(eye(4)-K*H)'+K*R*K'; 维度：4*4
// 中间变量：P_1=(eye(4)-K*H) P_1_z=(eye(4)-K*H)' P_2=K*R*K'
// 计算 P_1=(eye(4)-K*H) 维度：4*2 * 2*4=4*4
for (p=0;p<4;p++)
{
for (q=0;q<4;q++)
{
if (p==q)
P_1[p][q]=1-(K[p][0]*H[0][q]+K[p][1]*H[1][q]);
else
P_1[p][q]=0-(K[p][0]*H[0][q]+K[p][1]*H[1][q]);
/// printf("P_1[%d][%d] is %.4f\n",p,q,P_1[p][q]); //前两个正确
}
}
// 计算 P_1_est=(eye(4)-K*H)*P_pre = P_1*P_pre 维度：4*4
for (p=0;p<4;p++)
{
for (q=0;q<4;q++)
{
for (r=0;r<4;r++)
{
P_1_est[p][q]=P_1_est[p][q]+P_1[p][r]*P_pre[r][q];
}
//printf("P_1_est[%d][%d] is %.4f\n",p,q,P_1_est[p][q]); //前两个正确
}
}
//计算P_2_L=K*R 维度：4*2
for (p=0;p<4;p++)
{
for (q=0;q<2;q++)
{
for(r=0;r<2;r++)
{
P_2_L[p][q]=P_2_L[p][q]+K[p][r]*R[r][q];
}
//printf("P_2_L[%d][%d] is %.4f\n",p,q,P_2_L[p][q]);//前两个正确
}
}
//计算K_z 维度：2*4
for (p=0;p<2;p++)
{
for (q=0;q<4;q++)
{
K_z[p][q]=K[q][p];
// printf("K_z[%d][%d] is %.4f\n",p,q,K_z[p][q]);
}
}
// 计算 P_2=K*R*K'=P_2_L*K' 维度：4*2 * 2*4=4*4
for (p=0;p<4;p++)
{
for (q=0;q<4;q++)
{
P_2[p][q]=P_2_L[p][0]*K_z[0][q]+P_2_L[p][1]*K_z[1][q];
//printf("P_2[%d][%d] is %.4f\n",p,q,P_2[p][q]);
}
}
// 计算 P_1_z=(eye(4)-K*H)' =P_1' 维度：4*4
for (p=0;p<4;p++)
{
for (q=0;q<4;q++)
{
P_1_z[p][q]=P_1[q][p];
}
}
// 计算P_est= (eye(4)-K*H)*P_pre *(eye(4)-K*H)'+K*R*K'
// =P_1*P_pre*P_1_z +P_2 = P_1_est*P_1_z+P_2 维度：4*4
for (p=0;p<4;p++)
{
for (q=0;q<4;q++)
{
for (r=0;r<4;r++)
{
P_est[p][q]=P_est[p][q]+P_1_est[p][r]*P_1_z[r][q]+P_2[p][q]/4.0;// 因为r循环了4次，所以要除以4
}
//printf("P_est[%d][%d] is %.4f\n",p,q,P_est[p][q]);
}
}
printf("\n\r");
//printf("P_est[0][0] is %.4f\n",P_est[0][0]);
/*--------------预测----------------*/
// 下一时刻状态的预测值 X_pre== F*X_est; 维度：4*1
// 使用之前清零，保证下一次计算正确
for (p=0;p<4;p++)
{
X_pre[p][0]=0;
}
for (p=0;p<4;p++)
{
for (r=0;r<4;r++)
{
X_pre[p][0]=X_pre[p][0]+F[p][r]*X_est[r][0];
}
//printf("X_pre[%d][0] is %.4f\n",p,X_pre[p][0]);
}
//预测状态误差自相关矩阵 相当于P（n+1,n） P_pre= F*P_est*F'+Q;
// 计算F*P_est=F_P 维度：4*4
for (p=0;p<4;p++)
{
for (q=0;q<4;q++)
{
for (r=0;r<4;r++)
{
F_P[p][q]=F_P[p][q]+F[p][r]*P_est[r][q];
}
}
}
// 计算F' 维度：4*4
for (p=0;p<4;p++)
{
for (q=0;q<4;q++)
{
F_z[p][q]=F[q][p];
}
}
// 计算P_pre= F*P_est*F'+Q=F_P*F'+Q;
// 使用之前清零，保证下一次计算正确
for (p=0;p<4;p++)
{
for (q=0;q<4;q++)
{
P_pre[p][q]=0;
}
}
for (p=0;p<4;p++)
{
for (q=0;q<4;q++)
{
for (r=0;r<4;r++)
{
P_pre[p][q]=P_pre[p][q]+F_P[p][r]*F_z[r][q]+Q[p][q]/4.0;// 因为r循环了4次，所以要除以4
}
//printf("P_pre[%d][%d] is %.4f\n",p,q,P_pre[p][q]);
}
}
//将待返回的参数放在结构体
for (p=0;p<4;p++)
{
KEM_back_parameter.X_est[p][0] = X_est[p][0];// 返回状态估计值
KEM_back_parameter.X_pre[p][0] = X_pre[p][0];// 返回状态预测值
}
for (p=0;p<4;p++)
{
for (q=0;q<4;q++)
{
KEM_back_parameter.P_pre[p][q] = P_pre[p][q];//返回状态估计协方差矩阵的预测值
}
}
return KEM_back_parameter;
}
3 结果对比
