【Kalman学习笔记01】matlab仿真

最新推荐文章于 2024-08-25 12:56:41 发布

Springkiss

最新推荐文章于 2024-08-25 12:56:41 发布

阅读量495

点赞数

分类专栏： fpga算法 matlab 文章标签： matlab 算法开发语言

本文链接：https://blog.csdn.net/Springkiss/article/details/128172551

版权

fpga算法同时被 2 个专栏收录

1 篇文章 0 订阅

订阅专栏

matlab

1 篇文章 0 订阅

订阅专栏

1.卡尔曼滤波算法概要

卡尔曼滤波算法是一种基础预测定位算法，本质上就是通过预测和更新两个状态过程的迭代，来逐步的准确定位。

预测：当前状态环境下，对下一个时间段t的位置估计计算的值。

更新：根据传感器获取到的比较准确的位置信息后，来更新当前的预测位置，也就是纠正预测的错误。

卡尔曼算法依赖于线性计算和高斯分布。

算法的五大公式为：

$\large \overline{\hat x_t}=F\hat x_{t-1}+Bu_{t-1}$

$\large \overline{P_t}=FP_{t-1}F^T+Q$

$\large \overline{K_t}=\overline{P_t}H^T(H\overline{P_T}H^T+R)^{-1}$

$\large \hat{x_t}=\overline{\hat{x_t}}+K_t(z_t-H\overline{\hat{x_t}})$

$\large P_t=(I-K_tH)\overline{P_t}$

2.matlab代码理解

2.1产生方波

clear
clc
t = 1:628;
Z = square(t/50,50); %产生方波，周期为=pi*50*2=314
Z = Z+1.4;
Z = Z*80;
ZZ = randn(1,628);
ZZ = ZZ * 10;
Z = Z+ZZ;
plot(Z);

2.2打开数据文件

fid=fopen('test1_data.coe','w');
fprintf(fid,'memory_initialization_radix=2;\n');
fprintf(fid,'memory_initialization_vector=\n');

2.3量化数据

q = quantizer('fixed',[32,16]);
for i = 1:628
    y = num2bin(q, Z(i));
    y = num2str(y);
    fprintf(fid,'%s,\n',y);
end

2.3.1quantizer函数：

q=quantizer('datamode','roundmode','overflowmode','format')

1）DataMode：数据类型，默认是'fixed'。除默认还有4种。

fixed— 有符号定点模式。
ufixed— 无符号定点模式。
float— 自定义精度浮点模式。
single— 单精度模式。此模式将覆盖所有其他属性设置。
double— 双精度模式。此模式将覆盖所有其他属性设置。

2）RoundMode：取整模式，默认是floor。除默认还有5种

floor— 向下舍入到下一个允许的量化值。
convergent— 舍入到最接近的允许量化值。仅当舍入后的最低有效位设置为 0 时，恰好位于两个最接近的允许量化值之间的数字才会向上舍入。
fix— 向上舍入负数，将正数向下舍入到下一个允许的量化值。
ceil— 向上舍入到下一个允许的量化值。
nearest— 舍入到最接近的允许量化值。介于两个最接近的允许量化值之间的数字将向上舍入。
round— 舍入到最接近的允许量化值。介于两个最接近的允许量化值之间的数字将以绝对值向上舍入。

3）OverFlowMode：溢出如何处理，默认'saturate'，针对定点类型的参数，如果对浮点型设置了这个参数会没用，不会报错。

saturate— 溢出饱和。当要量化的数据值位于数据格式属性指定的最大和最小可表示数字的范围之外时，这些值将量化为最大或最小可表示值的值，具体取决于哪个值最接近。'wrap— 溢出换行到可表示值的范围。更换范围。超出动态范围的浮点数溢出到 ±Inf

4）Format：数据的格式，默认[16,15]。

对于fixed和ufixed是【数据全长，小数位长】

例如fixed，设置[8,6],数据全长8，小数位长6，符号位占1位，整数位占1位，所以能表示的数据范围是[-2,1.984375]。对应二进制'10000000' '01111111'。

2.3.2num2bin函数：

y＝num2bin（q，x）使用量化器对象q指定的数据类型属性将数字数组x转换为y中返回的二进制字符向量。如果x是包含数字矩阵的单元格数组，那么x将是包含二进制字符串的相同维度的单元格数组。如果x是一个结构，那么x的每个数字字段都被转换为二进制。
[y1，y2，…]=num2bin（q，x1，x2，…）将数字矩阵x1，x2，…转换为二进制字符串y1，y2…

2.3.3num2str函数：

s = num2str(A) 将数值数组转换为表示数字的字符数组。输出格式取决于原始值的量级。num2str 对使用数值为绘图添加标签和标题非常有用。

2.4关闭文件

y = num2bin(q, Z(100));
y = num2str(y);
fprintf(fid,'%s;\n',y);
fclose(fid);%关闭文件指针

2.5 滤波算法

将算法的五大公式简化后如下：

$X^- = X_{t-1}$

$P^-=P_{t-1}+Q$

$Kg=P^-*(P^-+R)^{-1}$

X=X^-+Kg*(Z(i)-X^-)

P=(1-Kg)*P^-

2.5.1矩阵初始化

data = zeros(1,628);
 
Q = 0.1;
R = 3;
X_last = 0;
P_last = 1;
Dx = 0;

2.5.2卡尔曼滤波

for i=1:628
    X_ = X_last;%预测
    %X_ = X_last+(X_last-X_last)*0.05;%预测
    P_ = P_last +Q;%预测协方差
    Kg = P_/(P_+R);%卡尔曼增益
    X = X_+Kg*(Z(i)-X_);%估算最优值
    P = (1-Kg)*P_;
    
    Dx = X_last;%%保存t-1时刻的X-
    P_last = P;
    X_last = X;
    disp(Kg)
    data(i) = X;
end
plot(t,data,t,Z);