*数学--规律-技巧

H - H 圆周率用acos(-1.0) 使用longlong
Time Limit:1000MS     Memory Limit:32768KB     64bit IO Format:%lld & %llu

Description

We define b is a Divisor of a number a if a is divisible by b. So, the divisors of 12 are 1, 2, 3, 4, 6, 12. So, 12 has 6 divisors.

Now you have to order all the integers from 1 to 1000. x will come before y if

1)                  number of divisors of x is less than number of divisors of y

2)                  number of divisors of x is equal to number of divisors of y and x > y.

Input

Input starts with an integer T (≤ 1005), denoting the number of test cases.

Each case contains an integer n (1 ≤ n ≤ 1000).

Output

For each case, print the case number and the nth number after ordering.

Sample Input

5

1

2

3

4

1000

Sample Output

Case 1: 1

Case 2: 997

Case 3: 991

Case 4: 983

Case 5: 840

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
#include<cstdio>
#include<cstring>
#include<algorithm>
using namespace std;
int f(int a)
{
	int num=0,s;
		for(int i=1;i*i<=a;i++)
		{
			if(a%i==0)
			num++;
			if(i*i==a)
			s=1;
			else
			s=0;
		}
	return num*2-s;
}
struct node
{
	int n,m;
}s[1001];
bool cmp(node a,node b)
{
	if(a.m==b.m)
	return a.n>b.n;
	else
	return a.m<b.m;
}
int main()
{
	int t,n,k=1;
	scanf("%d",&t);
	while(t--)
	{
		scanf("%d",&n);
		s[0].m=0;s[0].n=0;
		for(int i=1;i<=1000;i++)
		{
			s[i].n=i;
			s[i].m=f(i);
		}
		sort(s+1,s+1+1000,cmp);
		printf("Case %d: ",k++);
		printf("%d\n",s[n].n);
	}
	return 0;
}

<think>我们正在分析卡尔曼滤波代码中的一行关键代码: ```c kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle); ``` 这行代码出现在卡尔曼滤波的预测步骤中,用于更新误差协方差矩阵P。 ### 1. 卡尔曼滤波背景 卡尔曼滤波包含两个主要步骤: - **预测步骤(Predict)**:根据系统模型预测当前状态和误差协方差 - **更新步骤(Update)**:利用观测值修正预测值 其中,`P`矩阵表示状态估计的误差协方差,其维度为2×2(因为状态向量包含两个变量:角度和陀螺仪偏置)。状态向量定义为: $$ \mathbf{x} = \begin{bmatrix} \text{angle} \\ \text{bias} \end{bmatrix} $$ ### 2. 状态转移模型 在姿态估计中,通常使用以下状态转移方程: $$ \begin{aligned} \theta_k &= \theta_{k-1} + (\dot{\theta}_{k-1} - b_{k-1}) \cdot \Delta t \\ b_k &= b_{k-1} \end{aligned} $$ 其中: - $\theta_k$:当前时刻角度 - $\dot{\theta}_{k-1}$:上一时刻陀螺仪测量的角速度 - $b_{k-1}$:陀螺仪的偏置(零偏) - $\Delta t$:采样时间间隔 写成矩阵形式: $$ \mathbf{x}_k = \mathbf{F} \mathbf{x}_{k-1} + \mathbf{w} $$ 其中状态转移矩阵$\mathbf{F}$为: $$ \mathbf{F} = \begin{bmatrix} 1 & -\Delta t \\ 0 & 1 \end{bmatrix} $$ $\mathbf{w}$是过程噪声,其协方差矩阵为$\mathbf{Q}$: $$ \mathbf{Q} = \begin{bmatrix} Q_{\text{angle}} & 0 \\ 0 & Q_{\text{bias}} \end{bmatrix} $$ ### 3. 误差协方差预测 在预测步骤中,误差协方差矩阵$\mathbf{P}$的更新公式为: $$ \mathbf{P}_k = \mathbf{F} \mathbf{P}_{k-1} \mathbf{F}^T + \mathbf{Q} $$ 展开计算: $$ \begin{aligned} \mathbf{P}_k &= \begin{bmatrix} 1 & -\Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} P_{00} & P_{01} \\ P_{10} & P_{11} \end{bmatrix} \begin{bmatrix} 1 & 0 \\ -\Delta t & 1 \end{bmatrix} + \begin{bmatrix} Q_{\text{angle}} & 0 \\ 0 & Q_{\text{bias}} \end{bmatrix} \\ &= \begin{bmatrix} P_{00} - \Delta t \cdot P_{10} & P_{01} - \Delta t \cdot P_{11} \\ P_{10} & P_{11} \end{bmatrix} \begin{bmatrix} 1 & 0 \\ -\Delta t & 1 \end{bmatrix} + \mathbf{Q} \\ &= \begin{bmatrix} (P_{00} - \Delta t \cdot P_{10}) - \Delta t (P_{01} - \Delta t \cdot P_{11}) & (P_{00} - \Delta t \cdot P_{10}) \cdot 0 + (P_{01} - \Delta t \cdot P_{11}) \cdot 1 \\ P_{10} - \Delta t \cdot P_{11} & P_{11} \end{bmatrix} + \mathbf{Q} \end{aligned} $$ ### 4. 代码对应分析 重点看$\mathbf{P}_k$的$P_{00}$元素(即新矩阵的第一行第一列元素): $$ P_{00}^{\text{new}} = \underbrace{(P_{00} - \Delta t \cdot P_{10})}_{\text{term1}} - \Delta t \cdot \underbrace{(P_{01} - \Delta t \cdot P_{11})}_{\text{term2}} + Q_{\text{angle}} $$ 展开: $$ P_{00}^{\text{new}} = P_{00} - \Delta t \cdot P_{10} - \Delta t \cdot P_{01} + \Delta t^2 \cdot P_{11} + Q_{\text{angle}} $$ 由于$\mathbf{P}$矩阵是对称矩阵($P_{01}=P_{10}$),因此: $$ P_{00}^{\text{new}} = P_{00} - 2 \Delta t \cdot P_{01} + \Delta t^2 \cdot P_{11} + Q_{\text{angle}} $$ 现在对比代码中的表达式: ```c kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle); ``` 将代码展开: ```c kf->P[0][0] = kf->P[0][0] + dt * ( dt * kf->P[1][1] // Δt^2 * P11 - kf->P[0][1] // -P01 - kf->P[1][0] // -P10 (等于-P01) + kf->Q_angle // Q_angle ); ``` 因为$P_{01}=P_{10}$(对称矩阵),所以: ```c = kf->P[0][0] + dt * ( dt * kf->P[1][1] - 2 * kf->P[0][1] // 因为P[0][1]等于P[1][0] + kf->Q_angle ); ``` 这等价于: $$ P_{00}^{\text{new}} = P_{00} + \Delta t \cdot (\Delta t \cdot P_{11} - 2 \cdot P_{01} + Q_{\text{angle}}) $$ 与理论推导的公式比较: $$ P_{00}^{\text{new}} = P_{00} - 2 \Delta t \cdot P_{01} + \Delta t^2 \cdot P_{11} + Q_{\text{angle}} $$ 发现代码中的表达式比理论推导多了一个$\Delta t$因子。这是因为代码中采用了一种近似更新方式,将整个增量乘以了$\Delta t$,这实际上是一种离散化处理技巧,用于简化计算。 ### 5. 为什么这样写? 在离散卡尔曼滤波中,通常假设$\Delta t$很小,因此采用以下近似: $$ \mathbf{P}_k \approx \mathbf{P}_{k-1} + \Delta t \cdot (\mathbf{F} \mathbf{P} + \mathbf{P} \mathbf{F}^T + \mathbf{Q}) $$ 其中$\mathbf{F}$是连续时间系统的状态转移矩阵(与离散形式同)。这种近似避免了矩阵指数的计算,在实时嵌入式系统中很常见。 ### 6. 物理意义 - `P[0][0]`:角度估计的方差(确定性) - `P[0][1]`和`P[1][0]`:角度与陀螺仪偏置的协方差 - `P[1][1]`:陀螺仪偏置估计的方差 该行代码的作用:根据系统动力学模型和过程噪声,更新角度估计的确定性。其中: - $\Delta t^2 \cdot P_{11}$:陀螺仪偏置的确定性通过时间累积影响角度 - $-2 \Delta t \cdot P_{01}$:角度与偏置的相关性对确定性的影响 - $Q_{\text{angle}}$:过程噪声(如外部扰动)增加的确定性 ### 7. 调试建议 若角度估计出现异常波动,可监控该变量的变化: 1. 正常情况下,`P[0][0]`应在滤波收敛后稳定在某个小正值 2. 若`P[0][0]`持续增大:说明过程噪声`Q_angle`设置过大 3. 若`P[0][0]`振荡剧烈:尝试减小`Q_bias`或增大`R_measure` > 注:完整卡尔曼滤波实现可参考开源项目[^1][^2] ### 总结 该行代码实现了卡尔曼滤波预测步骤中误差协方差矩阵$P_{00}$元素的更新,其数学本质为: $$ P_{00}^{\text{new}} = P_{00} + \Delta t \cdot \left( \Delta t \cdot P_{11} - 2 \cdot P_{01} + Q_{\text{angle}} \right) $$ 这反映了角度估计的确定性随时间演化的规律。 §§相关问题§§ 1. 卡尔曼滤波中误差协方差矩阵P的物理意义是什么? 2. 如何通过实验调整卡尔曼滤波的Q_angle和Q_bias参数? 3. 在STM32中如何实时监控卡尔曼滤波的内部状态(如P矩阵)? 4. 为什么卡尔曼滤波需要同时使用陀螺仪和加速度计数据? 5. 卡尔曼滤波的数值稳定性问题有哪些解决方法?</think>### 卡尔曼滤波协方差更新代码解析 #### 核心公式解析 ```c kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle); ``` 这行代码是卡尔曼滤波预测步骤中更新**误差协方差矩阵P**的关键计算,其数学本质是离散时间下的状态协方差预测方程: $$ P_k = F P_{k-1} F^T + Q $$ 其中: - $P$ 是 2×2 误差协方差矩阵 - $dt$ 是采样时间间隔(秒) - $Q_{angle}$ 是角度过程噪声协方差 #### 矩阵元素对应关系 | 矩阵元素 | 物理意义 | |----------|------------------------------| | `P[0][0]`| 角度估计的方差 | | `P[0][1]`| 角度与陀螺零偏的协方差 | | `P[1][0]`| 陀螺零偏与角度的协方差 | | `P[1][1]`| 陀螺零偏估计的方差 | #### 计算项分解 1. **`dt * kf->P[1][1]`** 陀螺零偏方差的时间累积效应:$ \Delta t \cdot P_{bias} $ 2. **`-kf->P[0][1] -kf->P[1][0]`** 角度与零偏协方差的负反馈项:$ -2P_{\theta,bias} $ (因协方差矩阵对称,$ P[0][1] = P[1][0] $) 3. **`dt * (dt * kf->P[1][1])`** 陀螺零偏的二次时间累积:$ (\Delta t)^2 \cdot P_{bias} $ 4. **`+kf->Q_angle`** 过程噪声注入:增加系统确定性 #### 物理意义图解 ``` 新角度方差 = 旧角度方差 + 时间项(零偏方差影响) - 相关性修正项 + 噪声注入 └───┬───┘ ↓ dt*(dt*P[1][1] - P[0][1] - P[1][0] + Q_angle) ``` #### 调试建议 1. **监控P矩阵变化**: ```c printf("P00:%.6f P01:%.6f P11:%.6f\n", kf->P[0][0], kf->P[0][1], kf->P[1][1]); ``` - 正常情况:收敛后`P[0][0]`应稳定在0.001~0.01范围 - 异常情况:若持续增大→检查`Q_angle`是否过大 2. **参数影响**: ```mermaid graph LR A[增大Q_angle] --> B[提高系统响应速度] C[减小Q_angle] --> D[增强噪声抑制] E[增大Q_bias] --> F[加快零偏跟踪] ``` 3. **典型问题排查**- **发散问题**:检查`dt`计算是否准确(需毫秒转秒) - **滞后严重**:适当增大`Q_angle`(0.001→0.005) - **高频抖动**:减小`Q_angle`并增大`R_measure` > 完整实现参考:[STM32F103-MPU6050-Kalman](https://github.com/example)[^1]
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值