size_t类型 zz

本文详细介绍了size_t类型,一种用于表示内存大小的无符号整数类型。解释了其在不同操作系统(如Windows和OpenSolaris)中的定义方式,并强调了size_t在数组处理和sizeof操作符返回值中的应用。

http://blog.youkuaiyun.com/refiner/archive/2006/09/13/1216096.aspx

 

size_t是一种无符号的整型数,它的取值没有负数,在数组中也用不到负数,而它的取值范围是整型数的双倍。sizeof操作符的结果类型是size_t,它在头文件中typedefunsigned int类型。该类型保证能容纳实现所建立的最大对象的字节大小。

 

Windows:

size_t unsigned integer, Result of sizeof operator.  STDDEF.H and other include files

 

string.h和stddef.h中如下定义

 

OpenSolaris:

xref: /onnv/onnv-gate/usr/src/head/iso/stdlib_iso.h 中定义如下:

 

uint8_t vq = 0, vw = 0, ve = 0; uint8_t RxBuf[8]; char qa[100]; char qw[100]; uint8_t wei,df; static int m=0,f=0,s=0; uint8_t max,min; int op[100]={204,95,186,110,85,235,219,126,34,61,100,10,189,221,54,99,108,112,210,119,17,14,215,185,156,231,72,106,60,228,174,243,19,53,42,101,69,27,111,96,38,37,77,76,83,22,117,67,120,201,205,244,151,188,183,208,190,13,226,74,130,254,6,229,172,87,197,92,149,220,65,79,152,253,26,18,178,39,21,107,198,252,102,154,103,70,138,207,191,166,124,162,56,200,137,242,182,55,214,50}; float zz; int duianjian() { int rrt=0; //k1 if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_0) == GPIO_PIN_RESET) { HAL_Delay(10); if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_0) == GPIO_PIN_RESET) { rrt=1; return rrt; } } //k2 if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_1) == GPIO_PIN_RESET) { HAL_Delay(10); if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_1) == GPIO_PIN_RESET) { rrt=2; return rrt; } } return rrt; }void paixu(int rr[], int ze) { for (int i = 0; i < ze - 1; i++) { for (int j = 0; j < ze - i - 1; j++) { if (rr[j] > rr[j + 1]) { int temp = rr[j]; rr[j] = rr[j + 1]; rr[j + 1] = temp; } } } } //zhongweishu float zhongweishu(int rr[], int ze) { paixu(rr, ze); if (ze % 2 == 0) { return (rr[ze / 2 - 1] + rr[ze / 2]) / 2.0f; } else { return rr[ze / 2]; } } while (1) { /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ uint8_t i; i=duianjian(); if(i>0) { if(i==1) { zz=zhongweishu(op,100); sprintf(qw,"zhongweishu: %f\n", zz); HAL_UART_Transmit(&huart1, (uint8_t *)qw,strlen((uint8_t *)qw), 1000); } if(i==2) { paixu(op,100); int r=op[0],t=op[100]; sprintf(qw,"max: %d\nmin:%d\n", r,t); HAL_UART_Transmit(&huart1, (uint8_t *)qw,strlen((uint8_t *)qw), 1000); } }
07-03
#ifndef __UNSCENTED_KALMAN_FILTER_HPP__ #define __UNSCENTED_KALMAN_FILTER_HPP__ #include <Eigen/Dense> #include <opencv2/opencv.hpp> #include <opencv2/core/eigen.hpp> #include <iostream> #include <vector> #include <cmath> using namespace Eigen; using namespace std; template<int V_Z = 1, int V_X = 3, int n = 3> class UkfBuff { public: // 状态维度和观测维度 static const int state_dim = V_X; static const int obs_dim = V_Z; // 类型别名 using Mat_zz = Matrix<float, V_Z, V_Z>; using Mat_xx = Matrix<float, V_X, V_X>; using Mat_zx = Matrix<float, V_Z, V_X>; using Mat_xz = Matrix<float, V_X, V_Z>; using Mat_x1 = Matrix<float, V_X, 1>; using Mat_z1 = Matrix<float, V_Z, 1>; using Mat_xn = Matrix<float, V_X, 2 * n + 1>; using Mat_zn = Matrix<float, V_Z, 2 * n + 1>; // 状态转移矩阵(可选) Mat_xx F; // UKF 核心变量 Mat_x1 x_k1; // 状态估计 Mat_x1 P_x_k1; // 预测状态均值 Mat_z1 z_k1; // 观测预测均值 Mat_xz K; // 卡尔曼增益 Mat_zx H; // 观测矩阵(线性化时使用) Mat_xx Q; // 过程噪声协方差 Mat_zz R; // 观测噪声协方差 Mat_xx P; // 状态协方差 Mat_xx P_; // 预测状态协方差 Mat_zz Pz; // 观测协方差 Mat_xn x; // Sigma 点集合(状态) Mat_zn z; // Sigma 点集合(观测) // 权重 float Wc[2 * n + 1]; float Wm[2 * n + 1]; float alpha = 0.01; int beta = 2; float kappa = 3 - n; float lambda = alpha * alpha * (n + kappa) - n; // 自适应参数 int window_size; vector<Mat_z1> innovation_window; // 新息窗口 // 构造函数 UkfBuff(const std::string& params_path) { load_params(params_path); x_k1 << 0.9125, 1.942, 0; // 初始状态 } // 从 YAML 文件加载参数 void load_params(const std::string& params_path) { cv::FileStorage fs(params_path, cv::FileStorage::READ); if (!fs.isOpened()) { cerr << "无法打开配置文件: " << params_path << endl; return; } cv::Mat process_noise, measurement_noise, state_cov; // 读取窗口大小 fs["UKFBUFF"]["window_size"] >> window_size; // 读取过程噪声 fs["UKFBUFF"]["process_noise_cov"] >> process_noise; cv::cv2eigen(process_noise, Q); // 读取观测噪声 fs["UKFBUFF"]["measurement_noise_cov"] >> measurement_noise; cv::cv2eigen(measurement_noise, R); // 读取初始状态协方差 fs["UKFBUFF"]["state_cov"] >> state_cov; cv::cv2eigen(state_cov, P); fs.release(); cout << "UKF 滤波器加载正常" << endl; } // 状态转移函数(示例:恒等变换) Mat_x1 F_predict(Mat_x1 x_k1) { return x_k1; } // 观测函数(非线性) Mat_z1 H_(Mat_x1 x_k1, float t) { Mat_z1 z; z[0] = x_k1[0] * sin(x_k1[1] * t + x_k1[2]) + (2.09 - x_k1[0]); return z; } float V_pred(float t) { float pred_v = x_k1[0] * sin(x_k1[1] * t + x_k1[2]) + (2.09 - x_k1[0]); return pred_v; } // 重置 void reset(const std::string& params_path) { load_params(params_path); innovation_window.clear(); } // UKF 更新步骤 void update(float t, Mat_z1 z_k) { // 权重初始化 Wm[0] = lambda / (n + lambda); Wc[0] = Wm[0] + 1 - alpha * alpha + beta; for (int i = 1; i <= 2 * n; i++) { Wm[i] = 1 / (2 * (n + lambda)); Wc[i] = 1 / (2 * (n + lambda)); } // 预测 sigma 点 Mat_xx P1 = P.llt().matrixL(); P1 = sqrt(lambda + n) * P1; x.col(0) = F_predict(x_k1); for (int i = 1; i <= n; i++) { x.col(i) = F_predict(x_k1 + P1.col(i - 1)); x.col(i + n) = F_predict(x_k1 - P1.col(i - 1)); } // 更新先验状态均值 P_x_k1.setZero(); for (int i = 0; i <= 2 * n; i++) { P_x_k1 += Wm[i] * x.col(i); } // 更新先验协方差 P_ = Q; Mat_x1 x_differ; for (int i = 0; i <= 2 * n; i++) { x_differ = x.col(i) - P_x_k1; P_ += Wc[i] * x_differ * x_differ.transpose(); } // 观测 sigma 点 for (int i = 0; i <= 2 * n; i++) { z.col(i) = H_(x.col(i), t); } // 观测均值 z_k1.setZero(); for (int i = 0; i <= 2 * n; i++) { z_k1 += Wm[i] * z.col(i); } // 观测协方差 Pz = R; Mat_z1 z_differ; for (int i = 0; i <= 2 * n; i++) { z_differ = z.col(i) - z_k1; Pz += Wc[i] * z_differ * z_differ.transpose(); } // 交叉协方差 Mat_xz Pxy = Mat_xz::Zero(); for (int i = 0; i <= 2 * n; i++) { z_differ = z.col(i) - z_k1; x_differ = x.col(i) - P_x_k1; Pxy += Wc[i] * x_differ * z_differ.transpose(); } // 卡尔曼增益 K = Pxy * Pz.inverse(); // 新息 Mat_z1 z_measure_differ = z_k - z_k1; // 添加新息到窗口 innovation_window.push_back(z_measure_differ); if (innovation_window.size() > window_size) innovation_window.erase(innovation_window.begin()); // 自适应更新 R(观测噪声) if (innovation_window.size() == window_size) { Mat_zz R_new = Mat_zz::Zero(); for (auto& inno : innovation_window) { R_new += inno * inno.transpose(); } R = R_new / window_size*1e-1; }else{ R[0] = 1.5; } cout<<"R"<<R<<endl; // 更新状态 x_k1 = P_x_k1 + K * z_measure_differ; // 更新协方差 P = P_ - K * Pz * K.transpose(); } }; #endif 请参考matlab代码修改上面不对的地方,我发现我给的参数相同,但结果不一样,上面的不好% 清空工作区和命令窗口 clear ; clc; close all; % 读取CSV文件(请替换为实际文件路径) full_data = readmatrix('ukf1.csv'); time = full_data(:, 1); speed = full_data(:, 3); % 确保时间序列严格递增 [time, sort_idx] = sort(time); speed = speed(sort_idx); % 初始状态估计(A, ω, φ, v0)✅ 必须是列向量 x0 = [ (0.780+1.045)/2; (1.884+2)/2; 0.0 ]; % 初始状态协方差 P0 = 1e-2*diag([0.01,0.01,100]); % 初始过程噪声协方差 Q Q = 1e-8*diag([1, 1,1]); % % 初始观测噪声协方差 R R = 1; % UKF 参数 alpha = 1e-2; beta = 2; kappa = 0; % 创建 UKF 对象 ukf = unscentedKalmanFilter(... 'StateTransitionFcn', @stateTransitionFcn, ... 'MeasurementFcn', @measurementFcn, ... 'State', x0, ... 'StateCovariance', P0, ... 'ProcessNoise', Q, ... 'MeasurementNoise', R, ... 'HasAdditiveMeasurementNoise', true, ... 'Alpha', alpha, ... 'Beta', beta, ... 'Kappa', kappa); % 初始化变量 innovations = []; R_estimates = []; Q_estimates = []; estimated_params = []; fit_speed = []; window_size = 20; % 更新UKF for i = 1:length(time) % 获取当前时间点 current_time = time(i); % 预测(不传入时间参数) [x_pred, P_pred] = predict(ukf); % 计算新息(残差) y_measured = speed(i); y_pred = measurementFcn(x_pred, current_time); % 传递时间给测量函数 y_innovation = y_measured - y_pred; % 更新观测噪声 R(滑动窗口估计) innovations = [innovations; y_innovation]; if length(innovations) > window_size innovations(1) = []; % 移除旧值 end current_R = max(var(innovations)*1e-1, 1e-8); % 保证不为0 if length(innovations) < window_size current_R = 1; end ukf.MeasurementNoise = current_R; R_estimates = [R_estimates; current_R]; % % 更新过程噪声 Q(简单方法:根据状态预测误差估计) % x_true = ukf.State; % x_error = x_true - x_pred; % current_Q = max(x_error * x_error', 1e-8); % ukf.ProcessNoise = diag(diag(current_Q)); % 保持对角 % Q_estimates = [Q_estimates; diag(current_Q)]; % 校正(需要传入 current_time) ukf.State = correct(ukf, y_measured, current_time); % 存储结果 estimated_params(i, :) = ukf.State'; fit_speed(i) = measurementFcn(ukf.State, current_time); end % 绘图 figure; plot(time, speed, 'b', 'DisplayName', '原始速度'); hold on; plot(time, fit_speed, 'k-', 'LineWidth', 1.5, 'DisplayName', 'AUKF拟合'); legend show; xlabel('时间 (s)'); ylabel('速度 (rad/s)'); title('自适应UKF滤波结果'); grid on; % 噪声估计图 figure; subplot(2,1,1); plot(time, R_estimates, 'r'); title('观测噪声 R 估计'); xlabel('时间 (s)'); ylabel('R值'); grid on; % subplot(2,1,2); % plot(time, current_Q, 'g'); % title('过程噪声 Q 估计'); % xlabel('时间 (s)'); % ylabel('Q值'); % grid on; % 参数估计变化过程 figure; plot(time, estimated_params); legend('A', '\omega', '\phi', 'v0'); title('参数估计变化过程'); xlabel('时间 (s)'); ylabel('参数值'); grid on; %% 函数定义区域 % 状态转移函数(单位阵) function x = stateTransitionFcn(x) % 恒等状态转移函数(不改变状态) x = x(:); % 强制为列向量 end % 观测函数(注意:x 是第一个参数) function y = measurementFcn(x, t) A = x(1); omega = x(2); phi = x(3); % v0 = x(4); v0 = 2.09-x(1); y = A * sin(omega*t + phi) + v0; end
07-23
INFO: [HLS 214-291] Loop 'VITIS_LOOP_56_2' is marked as complete unroll implied by the pipeline pragma (src/SmoothProfileOnXAxisMeanOptimize.cpp:56:19) INFO: [HLS 214-291] Loop 'VITIS_LOOP_43_1' is marked as complete unroll implied by the pipeline pragma (src/SmoothProfileOnXAxisMeanOptimize.cpp:43:19) INFO: [HLS 214-186] Unrolling loop 'VITIS_LOOP_56_2' (src/SmoothProfileOnXAxisMeanOptimize.cpp:56:19) in function 'SmoothProfileOnXAxisMeanOptimize' completely with a factor of 11 (src/SmoothProfileOnXAxisMeanOptimize.cpp:15:0) INFO: [HLS 214-186] Unrolling loop 'VITIS_LOOP_43_1' (src/SmoothProfileOnXAxisMeanOptimize.cpp:43:19) in function 'SmoothProfileOnXAxisMeanOptimize' completely with a factor of 32 (src/SmoothProfileOnXAxisMeanOptimize.cpp:15:0) INFO: [HLS 214-248] Applying array_partition to '_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6buffer': Complete partitioning on dimension 1. (src/SmoothProfileOnXAxisMeanOptimize.cpp:24:0) INFO: [HLS 214-248] Applying array_partition to '_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE3win': Complete partitioning on dimension 1. (src/SmoothProfileOnXAxisMeanOptimize.cpp:27:0) INFO: [HLS 200-111] Finished Compiling Optimization and Transform: CPU user time: 0 seconds. CPU system time: 0 seconds. Elapsed time: 6.958 seconds; current allocated memory: 202.637 MB. INFO: [HLS 200-111] Finished Checking Pragmas: CPU user time: 0 seconds. CPU system time: 0 seconds. Elapsed time: 0.002 seconds; current allocated memory: 202.637 MB. INFO: [HLS 200-10] Starting code transformations ... INFO: [HLS 200-111] Finished Standard Transforms: CPU user time: 0 seconds. CPU system time: 0 seconds. Elapsed time: 0.051 seconds; current allocated memory: 208.652 MB. INFO: [HLS 200-10] Checking synthesizability ... WARNING: [SYNCHK 200-23] src/SmoothProfileOnXAxisMeanOptimize.cpp:100: variable-indexed range selection may cause suboptimal QoR. INFO: [SYNCHK 200-10] 0 error(s), 1 warning(s). INFO: [HLS 200-111] Finished Checking Synthesizability: CPU user time: 0 seconds. CPU system time: 0 seconds. Elapsed time: 0.06 seconds; current allocated memory: 210.320 MB. INFO: [XFORM 203-401] Performing if-conversion on hyperblock from (src/SmoothProfileOnXAxisMeanOptimize.cpp:16:9) to (src/SmoothProfileOnXAxisMeanOptimize.cpp:90:9) in function 'SmoothProfileOnXAxisMeanOptimize'... converting 13 basic blocks. INFO: [XFORM 203-401] Performing if-conversion on hyperblock from (src/SmoothProfileOnXAxisMeanOptimize.cpp:94:45) in function 'SmoothProfileOnXAxisMeanOptimize'... converting 7 basic blocks. INFO: [XFORM 203-401] Performing if-conversion on hyperblock from (src/SmoothProfileOnXAxisMeanOptimize.cpp:89:13) to (src/SmoothProfileOnXAxisMeanOptimize.cpp:101:1) in function 'SmoothProfileOnXAxisMeanOptimize'... converting 10 basic blocks. INFO: [XFORM 203-11] Balancing expressions in function 'SmoothProfileOnXAxisMeanOptimize' (src/SmoothProfileOnXAxisMeanOptimize.cpp:10:1)...20 expression(s) balanced. INFO: [HLS 200-111] Finished Loop, function and other optimizations: CPU user time: 0 seconds. CPU system time: 0 seconds. Elapsed time: 0.157 seconds; current allocated memory: 233.824 MB. INFO: [HLS 200-111] Finished Architecture Synthesis: CPU user time: 0 seconds. CPU system time: 0 seconds. Elapsed time: 0.088 seconds; current allocated memory: 233.828 MB. INFO: [HLS 200-10] Starting hardware synthesis ... INFO: [HLS 200-10] Synthesizing 'SmoothProfileOnXAxisMeanOptimize' ... INFO: [HLS 200-10] ---------------------------------------------------------------- INFO: [HLS 200-42] -- Implementing module 'SmoothProfileOnXAxisMeanOptimize' INFO: [HLS 200-10] ---------------------------------------------------------------- INFO: [SCHED 204-11] Starting scheduling ... INFO: [SCHED 204-61] Pipelining function 'SmoothProfileOnXAxisMeanOptimize'. INFO: [HLS 200-1470] Pipelining result : Target II = 1, Final II = 1, Depth = 81, function 'SmoothProfileOnXAxisMeanOptimize' INFO: [SCHED 204-11] Finished scheduling. INFO: [HLS 200-111] Finished Scheduling: CPU user time: 0 seconds. CPU system time: 0 seconds. Elapsed time: 0.319 seconds; current allocated memory: 238.062 MB. INFO: [BIND 205-100] Starting micro-architecture generation ... INFO: [BIND 205-101] Performing variable lifetime analysis. INFO: [BIND 205-101] Exploring resource sharing. INFO: [BIND 205-101] Binding ... INFO: [BIND 205-100] Finished micro-architecture generation. INFO: [HLS 200-111] Finished Binding: CPU user time: 0 seconds. CPU system time: 0 seconds. Elapsed time: 0.118 seconds; current allocated memory: 239.660 MB. INFO: [HLS 200-10] ---------------------------------------------------------------- INFO: [HLS 200-10] -- Generating RTL for module 'SmoothProfileOnXAxisMeanOptimize' INFO: [HLS 200-10] ---------------------------------------------------------------- WARNING: [RTGEN 206-101] Design contains AXI ports. Reset is fixed to synchronous and active low. INFO: [RTGEN 206-500] Setting interface mode on port 'SmoothProfileOnXAxisMeanOptimize/points_in_z' to 'axis' (register, both mode). INFO: [RTGEN 206-500] Setting interface mode on port 'SmoothProfileOnXAxisMeanOptimize/smoothed_z' to 'axis' (register, both mode). INFO: [RTGEN 206-500] Setting interface mode on port 'SmoothProfileOnXAxisMeanOptimize/mean_win_size' to 'ap_none'. INFO: [RTGEN 206-500] Setting interface mode on port 'SmoothProfileOnXAxisMeanOptimize/invalid_z' to 'ap_none'. INFO: [RTGEN 206-500] Setting interface mode on function 'SmoothProfileOnXAxisMeanOptimize' to 'ap_ctrl_none'. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_1' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_2' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_3' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_4' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_5' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_6' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_7' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_8' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_9' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_10' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_11' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_12' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_13' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_14' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_15' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_16' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_17' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_18' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_19' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_20' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE6bu_21' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_buffer' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_buffer_1' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_buffer_2' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_buffer_3' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_buffer_4' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_buffer_5' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_buffer_6' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_buffer_7' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_buffer_8' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_buffer_9' is power-on initialization. WARNING: [RTGEN 206-101] Register 'p_ZZ32SmoothProfileOnXAxisMeanOptimizeRN3hls6streamIfLi0EEES2_7ap_uintILi6EEfE3wi' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_19' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_8' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_5' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_4' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_3' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_2' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_1' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_0' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_18' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_17' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_16' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_15' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_14' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_13' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_12' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_11' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_10' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_9' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_7' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_hls_stream_float_0_hls_stream_float_0_ap_uint_6_float_wi_s_6' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_win' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_win_1' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_win_2' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_win_3' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_win_4' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_win_5' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_win_6' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_win_7' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_win_8' is power-on initialization. WARNING: [RTGEN 206-101] Register 'SmoothProfileOnXAxisMeanOptimize_stream_stream_ap_uint_6_float_win_9' is power-on initialization. INFO: [HLS 200-1030] Apply Unified Pipeline Control on module 'SmoothProfileOnXAxisMeanOptimize' pipeline 'SmoothProfileOnXAxisMeanOptimize' pipeline type 'function pipeline' INFO: [HLS 200-1552] Enabling free running pipeline (frp) architecture on pipeline 'SmoothProfileOnXAxisMeanOptimize' in module 'SmoothProfileOnXAxisMeanOptimize'. Estimated max control fanout for pipeline is 10267. WARNING: [RTGEN 206-101] Port 'SmoothProfileOnXAxisMeanOptimize/mean_win_size' has no fanin or fanout and is left dangling. Please use C simulation to confirm this function argument can be read from or written to. INFO: [RTGEN 206-100] Generating core module 'ddiv_64ns_64ns_64_59_no_dsp_1': 1 instance(s).
最新发布
07-29
#ifndef __UNSCENTED_KALMAN_FILTER_HPP__ #define __UNSCENTED_KALMAN_FILTER_HPP__ #include "Eigen/Dense" #include <math.h> #include <glog/logging.h> #include <tf2/convert.h> #include <Eigen/Core> #include <opencv2/core/eigen.hpp> #include <opencv2/opencv.hpp> #include <tf2/LinearMath/Quaternion.h> #include <tf2/LinearMath/Matrix3x3.h> #include <geometry_msgs/msg/transform_stamped.hpp> #include <select_utils.hpp> #include <math_utils.hpp> using namespace Eigen; using namespace std; template<int V_Z = 1, int V_X = 3, int n = 3> class UkfBuff { public: using Mat_zz = Matrix<float, V_Z, V_Z>; using Mat_xx = Matrix<float, V_X, V_X>; using Mat_zx = Matrix<float, V_Z, V_X>; using Mat_xz = Matrix<float, V_X, V_Z>; using Mat_x1 = Matrix<float, V_X, 1>; using Mat_z1 = Matrix<float, V_Z, 1>; using Mat_xn = Matrix<float, V_X, 2*n+1>; using Mat_zn = Matrix<float, V_Z, 2*n+1>; Mat_xx F; Mat_x1 x_k1; Mat_x1 P_x_k1; Mat_z1 z_k1; Mat_xz K; Mat_zx H; Mat_xx R; // 观测噪声 Mat_zz Q; // 过程噪声 Mat_xx P; // 状态协方差 Mat_xx P_; Mat_zz Pz; Mat_xx Px; Mat_xn x; Mat_zn z; float Wc[2*n+1]; float Wm[2*n+1]; float alpha = 0.01; int belta = 2; float kappa = 3 - n; float lanmuda = alpha * alpha * (n + kappa) - n; // 自适应相关参数 int window_size; vector<Mat_z1> innovation_window; // 新息窗口 UkfBuff(const std::string& params_path) { load_params(params_path); } void load_params(const std::string& params_path) { cv::FileStorage fs(params_path, cv::FileStorage::READ); cv::Mat process_noise, measurement_noise,state_cov; fs["UKFBUFF"]["window_size"] >> window_size; fs["UKFBUFF"]["process_noise_cov"] >> process_noise; fs["UKFBUFF"]["measurement_noise_cov"] >> measurement_noise; fs["UKFBUFF"]["state_cov"] >> state_cov; cv::cv2eigen(process_noise, Q); cv::cv2eigen(measurement_noise, R); cv::cv2eigen(state_cov, P); x_k1 << 0.9125, 1.942, 0; fs.release(); std::cout << "ukf滤波器加载正常" << std::endl; } Mat_x1 F_predict(Mat_x1 x_k1) { return x_k1; // 状态转移函数(恒等) } Mat_z1 H_(Mat_x1 x_k1, float t) { Mat_z1 z; z[0] = x_k1[0] * sin(x_k1[1] * t + x_k1[2]) + (2.09 - x_k1[0]); return z; } void reset(const std::string& params_path) { load_params(params_path); innovation_window.clear(); } void update(float t, Mat_z1 z_k) { // 权重初始化 Wm[0] = lanmuda / (n + lanmuda); Wc[0] = Wm[0] + 1 - alpha*alpha + belta; for(int i = 1; i <= 2*n; i++) { Wm[i] = 1 / (2 * (n + lanmuda)); Wc[i] = 1 / (2 * (n + lanmuda)); } // 预测 sigma 点 Mat_xx P1; x.fill(0); x.col(0) = F_predict(x_k1); P1 = P.llt().matrixL(); P1 = sqrt(lanmuda + n) * P1; for(int i = 1; i <= n; i++) { x.col(i) = F_predict(x_k1 + P1.col(i-1)); x.col(i + n) = F_predict(x_k1 - P1.col(i-1)); } // 更新先验状态均值 P_x_k1.fill(0); for(int i = 0; i <= 2*n; i++) { P_x_k1 += Wm[i] * x.col(i); } // 更新先验协方差 P_ = Q; Mat_x1 x_differ; for(int i = 0; i <= 2*n; i++) { x_differ = x.col(i) - P_x_k1; P_ += Wc[i] * x_differ * x_differ.transpose(); } // 观测 sigma 点 z.fill(0); for(int i = 0; i <= 2*n; i++) { z.col(i) = H_(x.col(i), t); } // 观测均值 z_k1.fill(0); for(int i = 0; i <= 2*n; i++) { z_k1 += Wm[i] * z.col(i); } // 观测协方差 Pz = R; Mat_z1 z_differ; for(int i = 0; i <= 2*n; i++) { z_differ = z.col(i) - z_k1; Pz += Wc[i] * z_differ * z_differ.transpose(); } // 交叉协方差 Mat_xz Pxy; Pxy.fill(0); for(int i = 0; i <= 2*n; i++) { z_differ = z.col(i) - z_k1; x_differ = x.col(i) - P_x_k1; Pxy += Wc[i] * x_differ * z_differ.transpose(); } // 卡尔曼增益 K = Pxy * Pz.inverse(); // 新息 Mat_z1 z_measure_differ = z_k - z_k1; // 添加新息到窗口 innovation_window.push_back(z_measure_differ); if (innovation_window.size() > window_size) innovation_window.erase(innovation_window.begin()); // 自适应更新 R(观测噪声) if (innovation_window.size() >= window_size) { Mat_zz R_new = Mat_zz::Zero(); for (auto& inno : innovation_window) { R_new += inno * inno.transpose(); } R = R_new / window_size; } // 更新状态 x_k1 = P_x_k1 + K * z_measure_differ; // 更新协方差 P = P_ - K * Pz * K.transpose(); } }; #endif 请检查这个代码的定义和处理,它好像把过程噪声和状态噪声弄混了
07-23
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值